四舵轮车辆中的舵轮角度计算

对于四舵轮车辆，或者对角线安装的双舵轮车辆来说，当同时存在线速度与角速度的时候，它的两个轮子的角度值是不一样的，而它的角度值与其当时的瞬心相关（机器人模型与ICR(Instantaneous Center of Rotation)）。关于这块的计算最近也花时间理解了一下，做个笔记。

1、瞬心的坐标计算

首先在瞬心的学习中可以知道对于四舵轮车辆，它的瞬心位置是任意的，但是满足以下条件：
1、瞬心到车体中心的距离满足下式：
$$r_c=\frac{v_c}{w}(w\ne 0)$$
2、瞬心的角度与速度的朝向垂直
3、瞬心的位置与角速度的方向相关，当角速度为正时，瞬心的角度为速度方向逆时针旋转90度，当角速度为负时，瞬心的角度为速度方向顺时针旋转90度。
关于第二点的理解再于：当计算某一时刻的瞬心时，默认小车在这一时刻是一个完全的刚体，其按照一定的线速度角速度做圆周运动，则其圆周运动的半径为$r_c$且垂直当前速度方向。
摘录一张示意图：

通过上述的约束，对于任意时刻当已知线速度与角速度的情况，我们可以计算出瞬心的坐标：
$$\theta =arctan\frac{v_x}{v_y}\pm \frac{\pi }{2}$$
$$R_c=\frac{v_c}{w}(w\ne 0)$$
$$\begin{array}{r}ic{c}_x=R_c\ast sin\theta \\ ic{c}_y=R_c\ast cos\theta \end{array}$$
函数实现：

// 计算速度向量角
double vel_theta = std::atan2(linear_velocity_y, linear_velocity_x);
// 转成瞬心的角度
vel_theta += (angular_velocity > 0) ? (3.1415926 / 2) : -(3.1415926 / 2);
//半径
double R = fabs(v/angular_velocity);
// 计算瞬心位置的半径
double icc_x = R*cos(vel_theta);
// 计算瞬心的y坐标
double icc_y = R*sin(vel_theta);
// 假设车辆中心在原点(0, 0)，瞬心在车辆的左侧或右侧
icc_position.point.x = icc_x;
icc_position.point.y = icc_y;
icc_position.point.z = 0;

2、舵轮角度计算

按照上式知道了舵轮的瞬心后，如何求出每个舵轮的角度？这个问题，思考了许久后发现它可以通过向量法快速的求解出来：

还是回到上面的图中，以车体中心建立机器人坐标系，假设四个轮子的位置分别为：($A_x$,$A_y$)、($B_x$,$B_y$)、($C_x$,$C_y$)、($D_x$,$D_y$)，瞬心的坐标为：($IC{R}_x$,$IC{R}_y$)。以瞬心为起点，四个轮子的坐标为终点作向量，即可以通过向量法求出四条线的角度：

$$\begin{array}{r}{\theta }_A=arctan\frac{IC{R}_y-A_y}{IC{R}_x-A_x}\\ {\theta }_B=arctan\frac{IC{R}_y-B_y}{IC{R}_x-B_x}\\ {\theta }_C=arctan\frac{IC{R}_y-C_y}{IC{R}_x-C_x}\\ {\theta }_D=arctan\frac{IC{R}_y-D_y}{IC{R}_x-D_x}\end{array}$$
然后，同样采用刚体的思想考虑每个轮子的状态，则在该时刻轮子的角度应与轮子到瞬心连线垂直，即$\pm \frac{\pi }{2}$。取正取负仍然是由角速度决定，当角速度为逆时针时，则角度值应当加$\frac{\pi }{2}$，反之则减。

// 获取线速度和角速度
double linear_velocity = vel;
double angular_velocity = rad;
// 计算每个轮子的转向角度
//这里得到的是轮子指向瞬心的角度
double theta_FL = std::atan2(icc_y_ - FL_y, icc_x_ - FL_x);
double theta_FR = std::atan2(icc_y_ - FR_y, icc_x_ - FR_x);
double theta_BL = std::atan2(icc_y_ - BL_y, icc_x_ - BL_x);
double theta_BR = std::atan2(icc_y_ - BR_y, icc_x_ - BR_x);

//加减pi/2代表轮子的方向
if(rad>0)
{
    theta_FL -= 3.1415926/2;
    theta_FR -= 3.1415926/2;
    theta_BL -= 3.1415926/2;
    theta_BR -= 3.1415926/2;
}
else{
    theta_FL += 3.1415926/2;
    theta_FR += 3.1415926/2;
    theta_BL += 3.1415926/2;
    theta_BR += 3.1415926/2;
}

3、完整测试

运行上述完整代码并发布对应位置的数据，为了直观表示，对于舵轮的角度采用pose表示，对于瞬心采用point表示，使用rviz可以得到如下测试结果：

图中红色框为当前机器人位置，绿色框为机器人的运动趋势，红色箭头则表示当前右前轮与左后轮两个轮子的计算角度，右侧紫色球体为当前的瞬心，由于原素材中Y方向没有速度，所以瞬心在Y轴上，但是总体还是能体现出瞬心与轮子的角度间的关系的。

参考：
四驱四转移动机器人运动模型及应用分析
常见移动机器人运动学模型总结
四轮驱动（SSMR）移动机器人运动模型及应用分析