四轮全向轮底盘运动学推导

code_killer_luke

已于 2023-11-25 23:16:28 修改

阅读量3.2k

点赞数 29

文章标签：机器人自动化

于 2023-11-25 23:14:28 首次发布

本文链接：https://blog.csdn.net/weixin_42722140/article/details/134621461

版权

四轮全向轮底盘运动学推导

符号定义

在这里插入图片描述

$\dot{X}$ ：底盘绝对坐标系下X方向速度

$\dot{Y}$ ：底盘绝对坐标系下Y方向速度

$u$ ：底盘在自身坐标系下x方向运动速度

$v$ ：底盘在自身坐标系下y方向运动速度

$\theta$ ：底盘自身坐标系x轴与绝对坐标系X轴的夹角

$\omega$ ：自转角速度

$L$ ：轮子到中心的距离

$v_1$ ~ $v_4$ ：四个轮子的速度

公式推导

$\vec{V}=\vec{u}+\vec{v}=\vec{\dot{X}}+\vec{\dot{Y}}$

$\left\{\begin{array}{c} \dot{X} \cos \theta+\dot{Y} \sin \theta=u \\ \dot{Y} \cos \theta-\dot{X} \sin \theta=v \\ \dot{\theta}=\omega \end{array}\right.$

$\left[\begin{array}{l} \mathrm{u} \\ \mathrm{v} \\ \omega \end{array}\right]=\left[\begin{array}{ccc} \cos \theta & \sin \theta & 0 \\ -\sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{c} \dot{X} \\ \dot{Y} \\ \dot{\theta} \end{array}\right]$

$\left\{\begin{array}{l} v_1=\frac{\sqrt{2}}{2} v-\frac{\sqrt{2}}{2} u+\omega L \\ v_2=-\frac{\sqrt{2}}{2} v-\frac{\sqrt{2}}{2} u+\omega L \\ v_3=-\frac{\sqrt{2}}{2} v+\frac{\sqrt{2}}{2} u+\omega L \\ v_4=\frac{\sqrt{2}}{2} v+\frac{\sqrt{2}}{2} u+\omega L \end{array}\right.$

$\left[\begin{array}{l} \mathrm{v}_1 \\ \mathrm{v}_2 \\ \mathrm{v}_3 \\ \mathrm{v}_4 \end{array}\right]=\left[\begin{array}{ccc} -\frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & L \\ -\frac{\sqrt{2}}{2} & -\frac{\sqrt{2}}{2} & L \\ \frac{\sqrt{2}}{2} & -\frac{\sqrt{2}}{2} & L \\ \frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & L \end{array}\right]\left[\begin{array}{l} u \\ v \\ \omega \end{array}\right]$

$=\frac{\sqrt{2}}{2}\left[\begin{array}{ccc} -1 & 1 & L \\ -1 & -1 & L \\ 1 & -1 & L \\ 1 & 1 & L \end{array}\right]\left[\begin{array}{l} u \\ v \\ \omega \end{array}\right]$

$=\frac{\sqrt{2}}{2}\left[\begin{array}{ccc} -1 & 1 & L \\ -1 & -1 & L \\ 1 & -1 & L \\ 1 & 1 & L \end{array}\right]\left[\begin{array}{ccc} \cos \theta & \sin \theta & 0 \\ -\sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{c} \dot{X} \\ \dot{Y} \\ \dot{\theta} \end{array}\right]$

$=\frac{\sqrt{2}}{2}\left[\begin{array}{ccc} -\cos \theta-\sin \theta & -\sin \theta+\cos \theta & L \\ -\cos \theta+\sin \theta & -\sin \theta-\cos \theta & L \\ \cos \theta+\sin \theta & \sin \theta-\cos \theta & L \\ -\cos \theta-\sin \theta & \sin \theta+\cos \theta & L \end{array}\right]\left[\begin{array}{c} \dot{X} \\ \dot{Y} \\ \dot{\theta} \end{array}\right]$

参考程序

void chassis_move(float target_speed, float target_dir, float target_omega)
{
    /********直接计算方法********/
    float speed_cal[4];
    float sin_ang = sin(chassis.angle);
    float cos_ang = cos(chassis.angle);
    float speed_X = target_speed * cos(target_dir);
    float speed_Y = target_speed * sin(target_dir);

    speed_cal[0] = ((-cos_ang - sin_ang) * speed_X + (-sin_ang + cos_ang) * speed_Y + chassis.Radius * target_omega) / sqrt(2);
    speed_cal[1] = ((-cos_ang + sin_ang) * speed_X + (-sin_ang - cos_ang) * speed_Y + chassis.Radius * target_omega) / sqrt(2);
    speed_cal[2] = ((cos_ang + sin_ang) * speed_X + (sin_ang - cos_ang) * speed_Y + chassis.Radius * target_omega) / sqrt(2);
    speed_cal[3] = ((cos_ang - sin_ang) * speed_X + (sin_ang + cos_ang) * speed_Y + chassis.Radius * target_omega) / sqrt(2);
    int speed_out[4] = {0, 0, 0, 0};
    for (int i = 0; i < 4; i++)
{
    speed_out[i] = (int)speed_cal[i];
    }
    chassis.SetMotorSpeed(speed_out);
}