tl;dr Built a JetBot + an exploration and novice implementation of motion planning on said JetBot. This computational game theory project marked my first foray into optimization and a glimpse of its power muahahaha. It ain’t exquisite but it was heading in the right direction.
Links
Background
Although autonomous vehicles are the talk of the town these days, the methodologies behind them are elusive to most. To help yourself, I suggest reading my note on LCPs and then follow that up with section 3.1 of this thesis on LMCPs and the path solver for MCPs.
Path planning in autonomous vehicles is a booming research area with significant developments. Although computer vision and machine learning are often employed to plan motion in autonomous vehicles, computationally solving the optimization problems, that arise from in scenarios of motion planning, through a game theoretic is a lightweight alternative to path solving. In this scenario, the path planning optimization problem is formulated as a nonlinear complementarity problem (NCP) constrained by physics and simple car dynamics, which cannot necessarily be directly and exactly solved. Instead the NCP can be approximated by linear mixed complementarity problems (LMCPs), iteratively computing partial paths that together approximate the solution to the NCP and yield a motion planned trajectory for an autonomous vehicle.
The problem formulation is a non-visual scenario where stationary obstacles are laid out on a grid, in a predetermined fashion, and an optimal path must be computed through these obstacles to some goal point without exceeding bounds. Such paths are often nonlinear and can be closely approximated by solving linear mixed complementarity problems via a pathsolver algorithm. Once the path is determined, the JetBot must then move in a real setting, of which the software representation of the grid space is a projection.
Building the JetBot
The JetBot was built following the documentation on the JetBot homepage. For the parts with multiple options: the IMX219-160 listed as the second option for cameras, the M2 card + antennas listed as the first option for wifi, and the 65mm wheels listed as the second option for wheels were used. The total cost was approximately $300. The hardware setup time was approximately twelve hours spread between two days. A significant portion of the time was spent extracting a screw terminal from the motor board that was placed incorrectly. 1 shows the completed JetBot hardware assembly.
Motion
Path Solver
This notebook contains code for non-visual motion planning – the primary objective of the project. The code relies on an LMCP solver that takes an LMCP formulation in M, q, l, u, x 0 and returns a path of points of z, w, v, t. A pathsolver then iteratively solves LMCPs for Newton points along an overarching path, performing backward linesearch to progress sufficiently down each of these paths towards the predefined goal point.
Solving many LMCPs approximates a nonlinear path, which can be formulated as an NMCP for which the KKT conditions must first be derived. The KKT conditions are formulated symbolically so that KKT function as well as the Jacobian of the KKT can be passed to the pathsolver for sparse JIT evaluation, accelerating runtime. Once the point sequence is acquired, it is passed to a module for JetBot motion planning to attempt to move the JetBot along the equivalent trajectory on a real grid space.
Motion Algorithms
To achieve the best motion possible on the JetBot, various motion planning algorithms were implemented: linear approximation, Manhattan (aka wiggle) motion, and proportional / integrative / derivative (PID) control. For some of these algorithms, the arctan2 function is used to compute the angle for turning from one orientation to another. (The full code for the JetBotMotion class is included in the appendix.)
arctan 2(∆y, ∆x)
The approximate relevant specifications measured for the JetBot were:
-
With two obstacles, sometimes the pathsolver fails if dt is too small =⇒ dt > 0.1
-
Confirmed that the solved states [x, y, v x , v y ] closely approximate the dynamics of horizontal motion
-
JetBot moves forward 40cm in 0.75 sec at speed=1
-
JetBot rotates 360 degrees in 1 sec at speed=1
Results
References
[1] Pepy, R., Lambert, A., and Mounier, H., “Path planning using a dynamic vehicle model,” in [2006 2nd International Conference on Information Communication Technologies], 1, 781–786 (2006). [2] Choset, H., La Civita, M., and Park, J., “Path planning between two points for a robot experiencing local- ization error in known and unknown environments,” (11 1999). [3] Lozano-Perez, T., “A simple motion-planning algorithm for general robot manipulators,” IEEE Journal on Robotics and Automation 3(3), 224–238 (1987). [4] Yonetani, R., Taniai, T., Barekatain, M., Nishimura, M., and Kanezaki, A., “Path planning using neural a* search,” in [International Conference on Machine Learning], 12029–12039, PMLR (2021). [5] Lee, L., Parisotto, E., Chaplot, D. S., Xing, E., and Salakhutdinov, R., “Gated path planning networks,” in [International Conference on Machine Learning], 2947–2955, PMLR (2018). [6] Mansouri, S. S., Kanellakis, C., Fresk, E., Kominiak, D., and Nikolakopoulos, G., “Cooperative coverage path planning for visual inspection,” Control Engineering Practice 74, 118–131 (2018). [7] Dirkse, S. and Ferris, M., “The path solver: A non-monotone stabilization scheme for mixed complementarity problems,” Optimization Methods and Software 5 (12 1993). [8] Andersson, J. A. E., Gillis, J., Horn, G., Rawlings, J. B., and Diehl, M., “CasADi – A software framework for nonlinear optimization and optimal control,” Mathematical Programming Computation (In Press, 2018). [9] Araki, M., “Pid control,” CONTROL SYSTEMS, ROBOTICS, AND AUTOMATION 2.