Trajectory Planning

Path Planning 

“path planning problem formulation involves searching the configuration space (C-space) of a robotic system for a collision-free path that connects a start configuration a goal configuration” (Bertram et al., 2006, p.1874).

Having implemented a suitable knee joint with simple end effector to target seeking behaviour, a method of imitating accurate movement of leg limbs during the swing phase is required.



Robotics and IK research papers both frequently discuss the application of interpolation between control points to plot desired trajectories: “interpolation is used to generate the foot trajectory in single support phase.” (Cuevas, Zaldívar and Rojas, 2004, p. 9).

This behaviour would dynamically plot unobstructed trajectories for feet (chain end effectors) through world space, accounting for obstacles and variations in the surface of traversal from its ‘start’ position as it enters its swing phase, to its ‘goal’ where the swing phase is concluded, and double support phase begins.

The analytical IK solution already implemented will simulate appropriate manipulation of the knee joint through the swing phase of the limb, mimicking a realistic bipedal gait.

Using this method as inspiration for my own solution, a curve could be projected from the end effector of the support limb as it becomes the swing limb. This path would allow for checks to be run to ensure that the target position is reachable by the chain. The curve will act as a raycast, checking for intersections with colliders along the potential limb trajectory.

Curve Analysis 

The application of a curve within my project is promising, though research has revealed a variety of curves being used to plot trajectory.

To accommodate dynamic and more complex curvatures, papers indicate use of a cubic curve (four control points) to accurately define the trajectory. This is due to the constraints that need to be considered when moving the swing leg (foot/knee clearance height, relative position to pelvis position, avoidance of obstacles and end target position).
I used this advice as the basis for research, though did not initially eliminate alternate curves from consideration.

I began by assessing methods of interpolation between control points.

As figure 1 shows, interpolating linearly between coordinate data points will produce a path which yields C⁰ continuity. This means that while the plotted curve is continuous, it will lack the order of parametric continuity required for a smooth plotted pathway. My solution requires a smooth curve to imitate natural human motion.

Figure 1: Comparison of Curve Types (Bishop, 2013)

The lowest order of parametric continuity required is C¹. Both polynomial and cubic curves exhibit C¹ continuity, meaning the first derivatives will be continuous, providing a smooth pathway for objects to follow.

As also shown in figure 1, polynomial interpolation may suffer from ‘overfitting’ which creates significant oscillations in the curve plotted between control points. While this maintains a smooth trajectory throughout the curve length, for my solution the fluctuations within the curve would likely result in very unnatural limb motions.

Piecewise cubic splines (C² continuity) which are comprised of several smaller cubic curves (with C¹ continuity) joined at their endpoints are numerically more stable, more efficient to compute and display fewer oscillations from the control point vectors.
Piecewise cubic splines would allow for more complex trajectories over greater distances while maintaining smoothness.

For my needs I expect cubic curves to offer sufficient accuracy for the comparatively simple trajectories I require, and the small number of control points (4, for cubic curves) should yield a natural curved path free of oscillations that is more straightforward to control.
If in later development it proves necessary to have more granular control over the end effector trajectory, I will adapt my implementation to chain cubic curves together to give the increased precision of a piecewise cubic spline

This curve analysis confirmed the contents of my earlier research. Confident that cubic curves are the most appropriate curve implementation for my needs, I focused and continued my research.

The following parametric representation is the base form of a cubic curve:

Figure 2: Parametric Representation of a Cubic Curve (Lengyel, 2012, p. 317)

Where a, b, c and d are control point vectors. Q(t) is the point along the curve which corresponds to the interpolation parameter t.

Further reading reduced the remaining options to the two most commonly evidenced cubic curves used for trajectory planning in robotics (inverse kinematics): Bézier Curve and Catmull-Rom spline.

In terms of my IK solution, the significant difference between resulting curves drawn from each method is their relationship to their defining control points.

Bézier curves are interpolation point values between their control points. This means that while their curvature always passes through the start and end vectors (control points 1 and 4 respectively), the interior pieces of the curve are defined by the interior control point positions but do not necessarily reproduce a physically similar configuration.
This relationship to the control points also makes a Bézier curve globally controlled. Global cubic splines “maintain C² continuity everywhere” (Lengyel, 2012, p.331), meaning moving a single control point will affect the entire curve.

Figure 3: Curve Interpolation and Approximation of Control Points (Sellarès, n.d.)

In contrast to this, a Catmull-Rom spline has a much more analogous relationship to its control points. The resulting curvature will always cross each of their control point positions - allowing for far greater accuracy over the trajectory.
Catmull-Rom splines are also controlled locally. If a single control point is altered “then only that piece of the curve and it’s immediate neighbours can be affected” (Lengyel, 2012, p. 331).


I predict that the more accurate control of a Catmull-Rom spline will prove more appropriate as part of my solution, though acknowledge that papers show the Bézier curves to be a more frequent choice in similar projects, with Catmull-Rom splines being applied when the obstacles require more complex trajectories to be passed successfully.
As at this stage I am not fully certain of the final nature of my project, I will implement both curves to assess their potential.

Path Planning 

Both these curves have different properties and overall attributes. To gain a better understanding and learn which is likely to perform more suitably in practical application I will implement a trajectory solution to compare the behaviours of a cubic Bézier curve and a Catmull-Rom spline.
The solution will need to allow the curves to be altered during run time via the manipulation of the curve control points.


References
Bertram, D., Kuffner, J., Dillmann, R. and Asfour, T. (2006). n Integrated Approach to Inverse Kinematics andPath Planning for Redundant Manipulators. Proceedings of the 2006 IEEE International Conference on Robotics and Automation, [online] Available at: https://www.ri.cmu.edu/pub_files/pub4/bertram_dominik_2006_1/bertram_dominik_2006_1.pdf [Accessed 10 Dec. 2018].

Bishop, J. (2013). Curve Fitting: Spline Interpolation. [video] Available at: https://www.youtube.com/watch?v=dxvmafuP9Wk [Accessed 10 Dec. 2018].

Cuevas, E., Zaldívar, D. and Rojas, R. (2004). Walking trajectory control of a biped robot. [online] Available at: https://pdfs.semanticscholar.org/a386/89a3e1ae7835324941dd6874b9c14ff2e512.pdf [Accessed 11 Dec. 2018].

Lengyel, E. (2012). Mathematics for 3D Game Programming and Computer Graphics. 3rd ed. pp.317 - 354.

Bibliography
Breen, D., Regli, W. and Peysakhov, M. (n.d.). Hermiteand Catmull-Rom Curves. Available at: https://www.cs.drexel.edu/~david/Classes/CS536/Lectures/L-04_Hermite_Catmull_Rom.6.pdf [Accessed 13 Dec. 2018].

Choi, J. and Elkaim, G. (2008). Bezier Curve for Trajectory Guidance. Proceedings of the World Congress on Engineering and Computer Science 2008, [online] pp.1 - 5. Available at: https://www.researchgate.net/publication/252576833_B_ezier_Curve_for_Trajectory_Guidance [Accessed 12 Dec. 2018].

Holmer, J. (2015). Unite 2015 - A coder's guide to spline-based procedural geometry. [video] Available at: https://www.youtube.com/watch?v=o9RK6O2kOKo [Accessed 13 Dec. 2018].

Ingersoll, B. and Ingersoll, K. (2016). UAV Path-Planning using Bézier Curves and aReceding Horizon Approach. [online] pp.1 - 12. Available at: https://arc.aiaa.org/doi/10.2514/6.2016-3675 [Accessed 13 Dec. 2018].

Ji-wung Choi, J., Renwick E. Curry, R. and Gabriel Hugh Elkaim, G. (2009). Obstacle Avoiding Real-Time Trajectory Generation and Control of Omnidirectional Vehicles. [online] pp.1 - 6. Available at: https://ieeexplore.ieee.org/abstract/document/5160683 [Accessed 12 Dec. 2018].

Mehdi, S., Choe, R. and Naira, N. (2015). Avoiding Multiple Collisions through Trajectory Replanningusing Piecewise B ́ezier Curves. 2015 IEEE 54th Annual Conference on Decision and Control (CDC), [online] pp.2755-2760. Available at: https://ieeexplore-ieee-org.uos.idm.oclc.org/stamp/stamp.jsp?tp=&arnumber=7402633&tag=1 [Accessed 13 Dec. 2018].

Moutadayne, M. (2002). Comparison of Polynomial Interpolation and Cubic Splines. [online] Maplesoft.com. Available at: https://www.maplesoft.com/applications/view.aspx?SID=4270&view=html [Accessed 13 Dec. 2018].

Rowland, T. (2018). "C^infty Function." From MathWorld--A Wolfram Web Resource. [online] Mathworld.wolfram.com. Available at: http://mathworld.wolfram.com/C-InfinityFunction.html [Accessed 12 Dec. 2018].

Sanmiya, J. (2015). Dual Cubic. Available at: https://drive.google.com/file/d/0BwNGTmrPN0JXSFFCbVBPY3F0U0E/view [Accessed 12 Dec. 2018].

Sellarès, T. (n.d.). Curves. Available at: http://ima.udg.edu/~sellares/MEG/CurvesMEG09.pdf [Accessed 13 Dec. 2018].

Twigg, C. (2003). Catmull-Rom splines. [online] pp.1 - 3. Available at: https://www.cs.cmu.edu/~fp/courses/graphics/asst5/catmullRom.pdf [Accessed 13 Dec. 2018].

Zhou, F., Song, B. and Tian, G. (2011). B ezier Curve Based Smooth Path Planning for Mobile Robot. Journal of Information & Computational Science 8: 12 (2011) 2441–2450, [online] pp.1 - 10. Available at: https://www.researchgate.net/publication/285739464_Bezier_curve_based_smooth_path_planning_for_mobile_robot [Accessed 11 Dec. 2018].