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，但针对RMU进行了一定的简化，请参阅 Bubble坐标系

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

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

数学推导

在 $O_{chassis\_xyz}$ 中:

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

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

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

弹丸在空中飞行时间 $t$

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

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

根据几何关系，有：

$\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$

则:

$\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}}$

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

功能实现

Note

运动补偿的相关代码位于 bubble_contrib/bubble_aming 模块下

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

目标点 $P_0(x_0, y_0, z_0)$ 信息通过识别器经由 PnP解算获得。 PnP解算将世界坐标系下目标的位姿信息转换为相机坐标系下：

$\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]$

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

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

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

实际使用的 $^{chassis}_{gimbal}T$ 为：

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

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

通过numpy的 numpy.poly1d 模块，求解含 $t$ 多项式 $\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$ 的解。

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

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

结语

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