四舵轮底盘运动解算
舵轮具备向任意方向产生速度的能力，因此舵轮底盘可以进行全向运动。
定义底盘坐标系如下，x为正前方，原点为底盘中心。
底盘的运动状态可以表示为 \[ \boldsymbol{V}=\begin{bmatrix} \vec{v}& \vec{\omega}\end{bmatrix} \] 其中\(v\)表示底盘在\(X-Y\)平面的线速度，\(\omega\)表示底盘绕自身中心自转的角速度。
令\(\theta=\arctan(\frac{l}{w})\)，\(\vec{r_i}\)为车体中心指向轮子中心的向量，\(|\vec{r_i}|=\frac{\sqrt{l^2+w^2}}{2}\)
我们将角速度\(\omega\)映射到四个轮子上，于是有 \[ \vec{v_{\omega i}}=\vec{\omega}\times \vec{r_i}=|\vec\omega||\vec{r_i}|(\cos(\theta_i),sin(\theta_i)) \] 其中 \[ \begin{bmatrix} \theta_1\\ \theta_2\\ \theta_3\\ \theta_4 \end{bmatrix}= \begin{bmatrix} \pi-\theta\\ \theta-\pi\\ -\theta\\ \theta \end{bmatrix} \]
将线速度\(v\)映射到四个轮子上，于是有 \[ \vec{v_{li}}=\vec{v}=\begin{bmatrix}v_x\\v_y\end{bmatrix} \] 把线速度的投影和角速度的投影相加，得到 \[ \vec{v_{i}}=\vec{v_{li}}+\vec{v_{\omega i}} \] 于是我们有 \[ \vec{v_i}=\begin{bmatrix} 1 & 0 & |r|\cos(\theta_i)\\ 0 & 1 & |r|\sin(\theta_i) \end{bmatrix} \begin{bmatrix} v_x\\ v_y\\ \omega \end{bmatrix} \]
将所得的矢量的模作为舵轮的速度，矢量的方向作为舵轮的方向即可。