Example program showing how to use the QP solver in a robot control problem. More...
#include <piksel/baseapp.hpp>#include <Eigen/Dense>#include <EigenOpt/quadratic_programming.hpp>#include <EigenOpt/kernel_projection.hpp>#include <memory>#include <deque>
Go to the source code of this file.
Detailed Description
Example program showing how to use the QP solver in a robot control problem.
We consider the case of a unicycle robot moving within a planar environment. Without entering too much into details, once we choose a point \( \bm{p} \) on the robot that does not lie on the wheel axis, we can express the velocity of such point as a function of the wheels velocities \(\dot{\bm{\phi}} \): \( \dot{\bm{p}} = \bm{J}\dot{\bm{\phi}} \) (the Jacobian \(\bm{J}\) is a function of the orientation of the robot). We can try to follow a given target \( \bm{t} \) using a simple proprtional control law: \( \dot{\bm{p}} = \lambda \left(\bm{t} - \bm{p}\right) \). While moving, it is however important not to exceed the operational limits of the robot. As an example, we might want to ensure that each wheel doesn't rotate faster than a certain limit \(\dot{\bm{\phi}}_{lim}\) in either direction.
While moving, the robot should avoid a set of obstacles. If we denote with \( \delta_i = d_i^2 = \left\|\bm{p}-\bm{c}_i\right\|^2\) the squared distance from the robot to the center of the \(i\)-th obstacle, we can find that the derivative of such quantity is
\begin{equation} \dot{\delta}_i = 2 \left(\bm{p}-\bm{c}_i\right)^T \dot{\bm{p}} \end{equation}
One way to avoid hitting the obstacle is to ensure that, when the distance drops below a given threshold \(d_{min}\), the derivative above is non-negative, since this would mean that the distance is either increasing or constant. In mathematical terms, we could require that for all objects \( \dot{\delta}_i \geq - \gamma \left(d_i - d_{min}\right) \). Note that when \( d_i \gg d_{min} \), the derivative \(\dot{\delta}_i\) can be negative, meaning that the distance can decrease almost freely. This is important because it means that when the robot is far away from an object, it is allowed to approach it without issue. Combining all information detailed above, one can introduce for each obstacle a constraint in the form:
\begin{equation} -\left(\bm{p}-\bm{c}_i\right)^T\bm{J}\dot{\bm{\phi}} \leq 2 \gamma \left(d_i - d_{min}\right) \end{equation}
The control problem can thus be formulated as a quadratic optimization:
\begin{equation} \min_{\dot{\bm{\phi}}} \left\|J\dot{\bm{\phi}} - \lambda \left(\bm{t} - \bm{p}\right)\right\|^2 \quad\text{subject to:}\quad \begin{cases} -\left(\bm{p}-\bm{c}_i\right)^T\bm{J}\dot{\bm{\phi}} \leq 2 \gamma \left(d_i - d_{min}\right) \quad\forall i=1,\cdots,N_{obs} \\ -\dot{\bm{\phi}}_{lim} \leq \dot{\bm{\phi}} \leq \dot{\bm{\phi}}_{lim} \end{cases} \end{equation}
- Note
- To compile this example, you need to download piksel. Move into the root project folder (where the main CMakeLists.txt file is located) and type git clone --recursive https://github.com/bernhardfritz/piksel.git
Definition in file example_pixel_robot.cpp.
