四舵轮运动学正解

​ 上述为4WIS（Four-Wheel Independent Steering）的运动学图，正运动学是输入为移动机器人转向角和车轮速度，输出为移动机器人全局框架位置的模型。根据四个航向电机的目标角度和对应的目标驱动轮的转速，可以通过运动学正解的方式求出四舵轮底盘在x，y方向上的平动速度，和绕z轴旋转的角速度。

​ 为了简化运动学模型，我们假设机器人的四个轮子均与地面有良好接触，且每个轮子上的负载相同。 （ 的值为(1,2,3,4)表示第 i 个驱动轮的合速度，满足平行四边形法则，

故：

$$V_i=\sqrt{V_{xi}^2+V_{yi}^2}\tag{1} \label{eq1}$$ $$L=L_r+L_f\tag{2}\label{2}$$

​ 针对1轮分析可得（详细的就是连接轮中心与车中心，有一个wr的速度进行分解得来的）

$$V_{x1}=\ V_1cos\delta_1=V_{x}-\ w\ \times\ \frac{W}{2}\tag{3}\label{eq3}$$ $$V_{y1}=\ V_1sin\delta_1=V_{y}+\ w\times L_f\tag{4}\label{eq4}$$

​ 同理可得

$$V_{x2}=\ V_2cos\delta_2=V_{x}+w\ \times\ \frac{W}{2}\tag{5}\label{eq5}$$ $$V_{y2}=\ V_2sin\delta_2=V_{y}+\ w\times L_f\tag{6}\label{ eq6}$$ $$V_{x3}=\ V_3cos\delta_3=V_{x}+w\ \times\ \frac{W}{2}\tag{7}\label{eq7}$$ $$V_{y3}=\ V_3sin\delta_3=V_{y}-\ w\times L_r\tag{8}\label{eq8}$$ $$V_{x4}=\ V_4cos\delta_4=V_{x}-w\ \times\ \frac{W}{2}\tag{9}\label{eq9}$$ $$V_{y4}=\ V_4sin\delta_4=V_{y}-w\times L_r\tag{10}\label{eq10}$$

​ 四舵轮的轮系坐标系与全局坐标系的关系为：

$$\left[\begin{matrix}\dot{X_w}\\ \dot{Y_w}\\ \dot{\theta}\\\end{matrix}\right]=M\ \left[\begin{matrix}V_x\\V_y\\w\\\end{matrix}\right]\tag{11}\label{eq11}$$

​ 其中，M为：

$$M=\left[\begin{matrix}cos(\theta) & -sin(\theta) & 0\\sin(\theta) & cos(\theta) & 0\\0 & 0 &1\\\end{matrix}\right]\tag{12}\label{eq12}$$

​ 由(4)~(10)可得：

$$A\ \left[\begin{matrix}V_x\\V_y\\w\\\end{matrix}\right]=B\ \left[\begin{matrix}V_1\\V_2\\V_3\\V_4\\\end{matrix}\right]\tag{13}$$

​ 得：

$$\left[\begin{matrix}V_x\\V_y\\w\\\end{matrix}\right]=A^{-1}B\ \left[\begin{matrix}V_1\\V_2\\V_3\\V_4\\\end{matrix}\right]\tag{14}$$

​ 其中，A，B为：

$$A=\ \left[\begin{matrix}1&0&\ -\frac{W}{2}\\0&1& L_f\\1&0&\frac{W}{2}\\0&1& L_f\\1&0& \frac{W}{2}\\0&1& -L_r\\1&0&-\frac{W}{2}\\0&1& -L_r\\\end{matrix}\right]，B=\ \left[\begin{matrix}c\delta_1&0&0&0\\s\delta_1&0&0&0\\0&c\delta_2&0&0\\0&s\delta_2&0&0\\0&0&c\delta_3&0\\0&0&s\delta_3&0\\0&0&0&c\delta_4\\0&0&0&s\delta_4\\\end{matrix}\right]\tag{15}$$

​ c()和是s()分别指三角函数 𝑐𝑜𝑠()和 𝑠𝑖𝑛()。四轮舵轮机器人的正向运动学方程展开如下：：

$$\left[\begin{matrix}V_x\\V_y\\w\\\end{matrix}\right]=\ \frac{1}{4}\ \left[\begin{matrix}c\delta_1&c\delta_2&c\delta_3&c\delta_4\\{s\delta}_1&{s\delta}_2&{s\delta}_3&{s\delta}_4\\M_1&M_2&M_3&M_4\\\end{matrix}\right] \left[\begin{matrix}V_1\\V_2\\V_3\\V_4\\\end{matrix}\right]\tag{16}$$

​ 其中，

$$\left[\begin{matrix}M_1\\M_2\\M_3\\M_4\\\end{matrix}\right]=\ \left[\begin{matrix}\frac{{L_f}sin\delta_1-\frac{W}{2}cos\delta_1}{{(L_f)}^2+{(\frac{W}{2})}^2}\\\frac{{L_f}sin\delta_2+\ \frac{W}{2}cos\delta_2}{{(L_f)}^2+{(\frac{W}{2})}^2}\\\frac{-{L_r}sin\delta_3+\ \frac{W}{2}cos\delta_3}{{(L_r)}^2+{(\frac{W}{2})}^2}\\\frac{-{L_r}sin\delta_4-\frac{W}{2}cos\delta_4}{{(L_r)}^2+{(\frac{W}{2})}^2}\\\end{matrix}\right]\tag{17}$$

​ 由（11）和（16）可知，四舵轮在全局坐标系中的正向运动学公式为：

$$\left[\begin{matrix}\dot{X_w}\\\dot{Y_w}\\\dot{\theta}\\\end{matrix}\right]=\ \frac{1}{4}\ \left[\begin{matrix}c\left(\delta_1+\ \theta\right)&c\left(\delta_2+\ \theta\right)&c\left(\delta_3+\ \theta\right)&c\left(\delta_4+\ \theta\right)\\s\left(\delta_1+\ \theta\right)&s\left(\delta_2+\ \theta\right)&s\left(\delta_3+\ \theta\right)&s\left(\delta_4+\ \theta\right)\\M_1&M_2&M_3&M_4\\\end{matrix}\right]\left[\begin{matrix}V_1\\V_2\\V_3\\V_4\\\end{matrix}\right]\tag{18}$$

​ 通过只在局部坐标X轴或者Y轴上的平移运动可知，有下面（19）成立：

$$\left[\begin{matrix}\dot{X_w}\\\dot{Y_w}\\\dot{\theta}\\\end{matrix}\right]=\left[\begin{matrix}cos(\theta) & 0\\-sin(\theta) & 0\\0 &1\\\end{matrix}\right]\left[\begin{matrix}V \\w \\\end{matrix}\right]\tag{19}$$

​ 其中，V为全局坐标系中，车的速度，w为全局坐标系中，车的角速度。可以通过对全局速度随时间的积分得到移动机器人在全局框架中的位置。

​ 前面正解的时候，说过全局坐标系与轮系坐标系的关系，所以此时我直接给出下面公式。（如果有不知道为什么的，可以了解一下坐标系的旋转平移变换)

$$\left[\begin{matrix}V_x\\V_y\\w\\\end{matrix}\right]={M^{-1}}\ \left[\begin{matrix}X_w\\Y_w\\\theta\\\end{matrix}\right]\tag{20}$$

​ 有:

$${M^{-1}}=\left[\begin{matrix}cos(\theta) & sin(\theta) & 0\\-sin(\theta) & cos(\theta) & 0\\0 & 0 &1\\\end{matrix}\right]\tag{21}$$

本篇文章受 CC BY-NC-SA 4.0 协议保护，转载请注明出处。