RMUC2022运动补偿
=================================================
:Authors:
    Ligcox, 
    Li
:Contact: ligcox@birdiebot.top
:Date: 2022/08/14
:Copyright: This document has been placed in the public domain.

概述
----------------------------
在RMU竞赛中，机器人自身的运动会对自瞄系统产生极大的偏差。该文档阐述了通过运动补偿模块，消除由于哨兵机器人自身底盘运动对自瞄系统产生的偏差。

流程
----------------------------
TODO

坐标系定义
----------------------------
.. note:: 坐标系定义原则上参考 `REP103 <https://www.ros.org/reps/rep-0103.html>`__，但针对RMU进行了一定的简化，请参阅 `Bubble坐标系 <guide/Bubble坐标系.html>`__  

云台坐标系通过\ :math:`yaw`\ 轴与\ :math:`pitch`\ 轴连接在底盘上，初始坐标系\ :math:`O_{gimbal\_xyz}`\。

其中：前方为\ :math:`{x}`\ 正方向，左方为\ :math:`{y}`\ 正方向，上侧为\ :math:`{z}`\ 正方向。重力加速度 :math:`g` 方向为 :math:`z` 轴负方向。


数学推导
----------------------------

在\ :math:`O_{chassis\_xyz}`\ 中:

:math:`O_{gimbal\_xyz}` 中空间目标点\ :math:`P_0(x_0, y_0, z_0)`\ 、\ :math:`\vec{v}_0 = [v_{x_0}, v_{y_0}, v_{z_0}]^\top`\在变换矩阵 :math:`^{chassis}_{gimbal}T` 作用下，
变换为 :math:`P{(p_x, p_y, p_z)}` 、\ :math:`\vec{v} = [vx, vy, vz]^\top`，空间向量\ :math:`\overrightarrow{O_{gimbal}P}=[p_x, p_y, p_z]^\top`。
其中：

.. math::
    ^{chassis}_{gimbal}T = \left[\begin{array}{ccc}  ^{chassis}_{gimbal}R & ^{chassis}_{gimbal}p \\  0 & 1 \end{array}\right]

弹丸初速度 :math:`||\vec{v}||` 通过裁判系统获得，一般地： :math:`||\vec{v}|| = ||\vec{v}_0|| = 30m/s` 

弹丸在空中飞行时间\ :math:`t` 

底盘速度 :math:`\vec{v}_{chassis} = (v_{chassis}, 0 ,0)`

经过修正后，云台偏转角度在底盘坐标系下的偏转角度为 :math:`\alpha_{yaw}` 和 :math:`\alpha_{pitch}`。


根据几何关系，有：

.. math::

    \overrightarrow{O_{gimbal}P} = \left[\begin{array}{ccc}
        p_x \\ p_y \\ p_z
    \end{array}\right] = 
    \left[\begin{array}{ccc}
        ||\vec{v}||t\cos \alpha_{pitch}  \cos \alpha_{yaw} \\
        ||\vec{v}||t\cos \alpha_{pitch}\sin \alpha_{yaw} + v_{chassis}t \\
        -||\vec{v}||t\sin \alpha_{pitch} - \frac{1}{2} gt^2 \\
    \end{array}\right]
    \Longrightarrow \\
    \frac{1}{4}g^2 t^4+(v_{chassis}^2-gp_z-||\vec{v}||^2)t^2 - 2p_y v_{chassis}t + ||\overrightarrow{O_{gimbal}P}||^2 = 0

则:

.. math::

    \alpha_{pitch} = \arcsin \frac{-p_z-\frac{1}{2}gt^2}{||\vec{v}||t}

    \alpha_{yaw} = \arcsin \frac{p_y - v_{chassis}t}{||\vec{v}||t \cos \alpha_{pitch}}

由于云台 :math:`yaw` 轴与 :math:`pitch` 轴刚性连接到底盘，云台在底盘坐标系下偏转角度 :math:`\alpha_{yaw}` 和 :math:`\alpha_{pitch}` 即为云台相对于当前位置偏转角度。

功能实现
------------------------------------------
.. note:: 运动补偿的相关代码位于 ``bubble_contrib/bubble_aming`` 模块下

变换矩阵 :math:`^{chassis}_{gimbal}T` 通过机器人URDF文件及当前偏转绝对角度通过tf tree获取。

目标点 :math:`P_0(x_0, y_0, z_0)` 信息通过识别器经由 `PnP解算 <https://docs.opencv.org/4.x/d5/d1f/calib3d_solvePnP.html>`__ 获得。
PnP解算将世界坐标系下目标的位姿信息转换为相机坐标系下：

.. math::

    \left[\begin{array}{c}X_{c} \\ Y_{c} \\ Z_{c} \\ 1\end{array}\right]=\left[\begin{array}{cccc}r_{11} & r_{12} & r_{13} & t_{x} \\ r_{21} & r_{22} & r_{23} & t_{y} \\ r_{31} & r_{32} & r_{33} & t_{z} \\ 0 & 0 & 0 & 1\end{array}\right]\left[\begin{array}{c}X_{w} \\ Y_{w} \\ Z_{w} \\ 1\end{array}\right]

对于实际使用的目标在 :math:`O_{gimbal\_xyz}` 下的位姿估计值，由于相机坐标系与云台坐标系存在平移偏移。
实现过程中，解算结果的 ``tvec`` 为目标在云台坐标系（更准确地说是在弹丸射出的摩擦轮位置坐标系）的位姿信息。
此外，在进行补偿的过程中， :math:`t` 为弹丸射出后击中目标的时间，而 :math:`O_{chassis\_xyz}` 原点位于机器人底盘的几何中心。
因此，实际求解过程中使用的 :math:`^{chassis}_{gimbal}T` 与数学推导中有一定的变化。

tf tree获取的 :math:`^{chassis}_{gimbal}T_{tf\_tree}` 为：

.. math::

    ^{chassis}_{gimbal}T_{tf\_tree} = \left[\begin{array}{ccc}  ^{chassis}_{gimbal}R & ^{chassis}_{gimbal}p \\  0 & 1 \end{array}\right]

实际使用的 :math:`^{chassis}_{gimbal}T` 为：

.. math::

    ^{chassis}_{gimbal}T_{tf\_tree} = \left[\begin{array}{ccc}  ^{chassis}_{gimbal}R^\top & 0 \\  0 & 1 \end{array}\right]

获得 :math:`^{chassis}_{gimbal}T` 后，
能够求解空间向量\ :math:`\overrightarrow{O_{gimbal}P}=[p_x, p_y, p_z]^\top` 。

通过numpy的 `numpy.poly1d <https://numpy.org/doc/stable/reference/generated/numpy.poly1d.html>`__ 模块，
求解含 :math:`t` 多项式 :math:`\frac{1}{4}g^2 t^4+(v_{chassis}^2-gp_z-||\vec{v}||^2)t^2 - 2p_y v_{chassis}t + ||\overrightarrow{O_{gimbal}P}||^2 = 0` 的解。

:math:`t` 应当包含4个解，刨除两个负数解后， :math:`t` 的剩余解分别弹丸在瞄准姿态和抛射姿态击中目标的时间。取其中较小的一个，即为所求时间 :math:`t` 。

一般的， :math:`\alpha_{yaw}` 和 :math:`\alpha_{pitch}` 值域均为 :math:`[-\frac{\pi}{2}, \frac{\pi}{2}]` ，因此使用 :math:`\arcsin` 求解。

结语
------------------
文档阐述了当前版本Bubble中运动补偿模块实现的过程，通过对向量 :math:`\overrightarrow{O_{gimbal}P}` 的修改，还能够消除云台运动、空气阻力等对弹丸飞行造成的影响。