差速轮运动学模型

机器人坐标系下的变换

运动特性为两轮差速驱动，其底部后方两个同构驱动轮的转动为其提供动力，前方的随动轮起支撑作用并不推动其运动，如下图两轮差速驱动示意图所示。

机器人的运动简化模型如图 4-1 所示，X 轴正方向为前进、Y 轴正方向为左平移、Z 轴正方向为逆时针。机器人两个轮子之间的间距为 D，机器人 X 轴和 Z 轴的速度分别为：Vx和Vz ，机器人左轮和右轮的速度分别为：Vl 和Vr。

假设机器人往一个左前的方向行进了一段距离，设机器人的右轮比左轮多走的距离近似为 K, 以机器人的轮子上的点作为参考点做延长参考线，可得：θ1=θ2 。由于这个Δt 很小，因此角度的变化量θ1 也很小，因此有近似公式：

θ2≈sin⁡(θ2)=KD

由数学分析可以得到下面的式子：

(1)K=(VR−VL)∗Δt,ω=θ1Δt

由上面的公式和式子可以求解出运动学正解的结果：机器人 X 轴方向速度

Vx=(Vl+Vr)/2

机器人 Z 轴方向速度：

Vz=（Vr−Vl）/D

由正解直接反推得出运动学逆解的结果：

机器人左轮的速度：

Vl=Vx−（Vz∗D）/2

机器人右轮的速度：

Vr=Vx+（Vz∗D）/2

实现

运动学正解

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
/**
 * @brief 轮子线速度到机器人坐标速度的变换
 *        Vx = (Vl + Vr) / 2
 *        Vz = (Vr - Vl) / D
 * @return Eigen::Vector3f 机器人坐标系下的速度
 */
Eigen::Vector3f MotorToRobotSpeed(void)
{
    Eigen::Vector3f robot_speed;

    robot_speed(0) = -0.5f * motor_line_speed_measure_(0) + 0.5f * motor_line_speed_measure_(1);
    robot_speed(1) = 0 * motor_line_speed_measure_(0) + 0 * motor_line_speed_measure_(1);
    robot_speed(2) = (0.5f / body_radius_) * (motor_line_speed_measure_(0) + motor_line_speed_measure_(1));

    return robot_speed;
}

运动学逆解

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
/**
 * @brief 机器人坐标速度到轮子线速度的变换
 *        Vl = -Vx + (Vz * D) / 2
 *        Vr = Vx + (Vz * D) / 2
 * @param robot_speed 机器人坐标系下的速度
 * @return None
 */
void RobotToMotorSpeed(Eigen::Vector3f& robot_speed)
{
    motor_line_speed_target_(0) =  -1 * robot_speed(0)  + 0 * robot_speed(1) + body_radius_ * robot_speed(2);
    motor_line_speed_target_(1) =   1 * robot_speed(0)  + 0 * robot_speed(1) + body_radius_ * robot_speed(2);

    if(motor_num_ >= 4) {
        motor_line_speed_target_(2) = motor_line_speed_target_(0);
        motor_line_speed_target_(3) = motor_line_speed_target_(1);
    }
}

坐标系旋转

全局坐标系到机器人坐标系的变换

一个平面坐标系逆时针旋转一个角度后得到另一个坐标系，则同一个点在这两个坐标系之间的几何关系如下：

由上图可得：

(2)x′=OB+BC=ODcos⁡θ+ADsin⁡θ=xcos⁡θ+ysin⁡θ

(3)y′=AE−CE=ADcos⁡θ−ODsin⁡θ=ycos⁡θ−xsin⁡θ

则反过来的关系如下：

(4)[xy]=[cos⁡θ−sin⁡θsin⁡θcos⁡θ][x′y′]

则反过来的关系如下：

(5)[xy]=[cos⁡θ−sin⁡θsin⁡θcos⁡θ][x′y′]

实现

正解

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
/**
 * @brief 世界坐标到机器人坐标的速度变换
 * @param global_speed 为 Global 坐标的速度向量 xyw
 * @return Eigen::Vector3f Robot 坐标的速度向量 xyw
 */
Eigen::Vector3f GlobalToRobotSpeed(Eigen::Vector3f& global_speed)
{
    Eigen::Vector3f robot_speed;
    Eigen::Matrix3f rotate_mat;

    rotate_mat <<  cos(global_coordinat_z_), sin(global_coordinat_z_), 0.0f,
                  -sin(global_coordinat_z_), cos(global_coordinat_z_), 0.0f,
                  0.0f,                      0.0f,                     1.0f;
    robot_speed = rotate_mat * global_speed;

    return robot_speed;
}

逆解

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
/**
 * @brief 机器人坐标到世界坐标的速度变换
 * @param robot_speed Robot 坐标的速度向量 xyw
 * @return Eigen::Vector3f Global 坐标的速度向量 xyw
 */
Eigen::Vector3f RobotToGlobalSpeed(Eigen::Vector3f& robot_speed)
{
    Eigen::Vector3f global_speed;
    Eigen::Matrix3f rotate_mat;

    rotate_mat << cos(global_coordinat_z_), -sin(global_coordinat_z_), 0.0f,
                  sin(global_coordinat_z_),  cos(global_coordinat_z_), 0.0f,
                  0.0f,                      0.0f,                     1.0f;
    global_speed = rotate_mat * robot_speed;

    return global_speed;
}

参考文献

https://www.guyuehome.com/33953
https://blog.csdn.net/LINEAR1024/article/details/104956774
https://www.guyuehome.com/8392