Authors in 2020@Haviland , 2021@Haviland , and 2022@Haviland outlined a method for Reactive Mobile Manipulation of wheeled robots via quadratic programming. It is designed to (1) allow the robot end-effector to track a desired pose, (2) maximize translational manipulability, (3) align mobile base orientation for better reach, (4) reactively avoid collision, and (5) balance the conflicts between these goals by introducing pose-tracking tolerance.
This post summarizes their results and related materials in 2017@Lynch with minor modification for quick reference.
To achieve the desired tasks at the same time in a unified fashion, the authors proposed the following quadratic programming formulation. \begin{equation} \begin{aligned} \min_{\mathbf{x}} \quad f_o(\mathbf{x}) &= \frac{1}{2}\mathbf{x}^T \mathcal{Q} \mathbf{x} + \mathcal{C}^T \mathbf{x} \\ \textrm{s.t.} \quad \mathcal{J} \mathbf{x} &= \mathbf{\nu} \\ \mathcal{A}\mathbf{x} &\leq \mathcal{B} \\ \mathbf{x}^- &\leq \mathbf{x} \leq \mathbf{x}^+ \end{aligned} \end{equation} Note some open-source QP solvers such as qpOASES take problems in the form of \begin{equation} \begin{aligned} \min_{x} \quad f_o(x) &= \frac{1}{2}x^T H x + x^T g \\ \textrm{s.t.} \quad \text{lb}_{Ax} &\leq A x \leq \text{ub}_{Ax} \\ \quad \text{lb}_{x} &\leq x \leq \text{ub}_{x} \end{aligned} \end{equation}
To write the proposed formulation explicitly into the form that open-source QP solvers take, one gets the cost function. $$ \label{eq:objective} f_o(\dot{q}_b, \dot{q}_a, \delta) = \frac{1}{2} \begin{bmatrix} \dot{q}_b \\ \dot{q}_a \\ \delta \end{bmatrix}^T \begin{bmatrix} \frac{1}{|| \mathbf{e} ||} & {} & {} \\ {} & \lambda_{\dot{q}_a} & {} \\ {} & {} & \lambda_{\delta} \end{bmatrix} \begin{bmatrix} \dot{q}_b \\ \dot{q}_a \\ \delta \end{bmatrix} + \begin{bmatrix} \small \begin{pmatrix} 0 & -k_{\epsilon} \theta_{\epsilon} \end{pmatrix} & -J_m & 0 \end{bmatrix} \begin{bmatrix} \dot{q}_b \\ \dot{q}_a \\ \delta \end{bmatrix} $$ Subject to inequality constraint, $$ \begin{bmatrix} \nu - \small \Delta \\ -\infty \\ \vdots \\ -\infty \end{bmatrix} \leq \begin{bmatrix} \begin{pmatrix}J_b & J_a \end{pmatrix} & I \,\, \\ J_{c_1} & 0 \,\, \\ \vdots & \vdots \,\, \\ J_{c_l} & 0 \,\, \end{bmatrix} \begin{bmatrix} \dot{q}_b \\ \dot{q}_a \\ \delta \end{bmatrix} \leq \begin{bmatrix} \nu + \small \Delta \\ \psi_1 \\ \vdots \\ \psi_l \end{bmatrix} $$ and boundary constraint, $$ \begin{bmatrix} \mathcal{S}^-_q \\ \delta^- \end{bmatrix} \leq \begin{bmatrix} \dot{q}_b \\ \dot{q}_a \\ \delta \end{bmatrix} \leq \begin{bmatrix} \mathcal{S}^+_q \\ \delta^+ \end{bmatrix} $$ where one can choose $$ \begin{split} \lambda_{\delta} &= 0 \\ \delta^- &= -\infty \\ \delta^+ &= +\infty \end{split} $$ to allow tasks such as collision avoidance to take higher priority than the end-effector pose-tracking.
In additional to the optimization notations,
While the above formulation solves for mobile base velocity in its minimum-dof coordinates, in practice, one may want to get the wheel velocity for direct hardware control.
For a differential driver robot, as shown in 2017@Lynch Sec. (13.3.1.4) or 2022@Haviland Eq. (4), there exists Jacobians that map between the mobile base minimum-dof base velocity and its wheel velocity: $$ \begin{bmatrix} v \\ \omega \end{bmatrix} = \begin{bmatrix} \frac{R}{2} & \frac{R}{2} \\ -\frac{R}{W} & \frac{R}{W} \end{bmatrix} \begin{bmatrix} \omega_L \\ \omega_R \end{bmatrix} $$ and its inverse $$ \begin{bmatrix} \omega_L \\ \omega_R \end{bmatrix} = \begin{bmatrix} \frac{1}{R} & -\frac{W}{2R} \\ \frac{1}{R} & \frac{W}{2R} \end{bmatrix} \begin{bmatrix} v \\ \omega \end{bmatrix} $$ where $\omega_L, \omega_R$ are the left and right wheel velocities, $R$ is the wheel radius, and $W$ is the distance between two wheels.
Sometimes it's desirable to add more control tasks to the optimization problem. For example, one may want to penalize joint acceleration and, with redundant dof, one may want the robot stay close to a desired posture. In that case, one can extend the objective function in $\eqref{eq:objective}$ into a new one. $$ f_{\mathcal{O}}(\dot{q}_b, \dot{q}_a, \delta) = f_o(\dot{q}_b, \dot{q}_a, \delta) + \frac{1}{2} || \dot{q}_{r} - \dot{q} || ^2_{\lambda_{\ddot{q}}} + \frac{1}{2} ||\dot{q}_a^d - \dot{q}_a||^2_{\lambda_{\dot{q}_a^d}} $$ where
Expanding the joint acceleration cost gives, $$ \begin{split} \frac{1}{2} \Big|\Big| \dot{q}_{r} - \dot{q} \Big|\Big| ^2_{\lambda_{\ddot{q}}} &= \frac{1}{2} \begin{pmatrix} \dot{q}_{r} - \dot{q} \end{pmatrix}^T \lambda_{\ddot{q}} \begin{pmatrix} \dot{q}_{r} - \dot{q} \end{pmatrix} = \frac{1}{2} \dot{q}^T \lambda_{\ddot{q}} \dot{q} - \dot{q}_{r}^T \lambda_{\ddot{q}} \dot{q} + \cancelto{0}{\frac{1}{2} \dot{q}_{r}^T \lambda_{\ddot{q}} \dot{q}_{r}} \\ &= \frac{1}{2} \begin{bmatrix} \dot{q} \\ \delta \end{bmatrix}^T \begin{bmatrix} \lambda_{\ddot{q}} & {} \\ {} & 0 \end{bmatrix} \begin{bmatrix} \dot{q} \\ \delta \end{bmatrix} + \begin{bmatrix} - \dot{q}_{r}^T \lambda_{\ddot{q}} & 0 \end{bmatrix} \begin{bmatrix} \dot{q} \\ \delta \end{bmatrix} \end{split} $$ where $\frac{1}{2} \dot{q}_{r}^T \lambda_{\ddot{q}} \dot{q}_{r}$ is out of the equation since it's not controlled by the optimization variable. Likewise, expanding the desired arm posture-tracking cost gives, $$ \frac{1}{2} ||\dot{q}_a^d - \dot{q}_a||^2_{\lambda_{\dot{q}_a^d}} = \frac{1}{2} \begin{bmatrix} \dot{q}_b \\ \dot{q}_a \\ \delta \end{bmatrix}^T \begin{bmatrix} 0 & {} & {} \\ {} & \lambda_{\dot{q}_a^d} & {} \\ {} & {} & 0 \end{bmatrix} \begin{bmatrix} \dot{q}_b \\ \dot{q}_a \\ \delta \end{bmatrix} + \begin{bmatrix} 0 & - {\dot{q}_a^d}^T \lambda_{\dot{q}_a^d} & 0 \end{bmatrix} \begin{bmatrix} \dot{q}_b \\ \dot{q}_a \\ \delta \end{bmatrix} $$