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

news2024/10/4 4:21:58

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

1、瞬心的坐标计算

首先在瞬心的学习中可以知道对于四舵轮车辆，它的瞬心位置是任意的，但是满足以下条件：
1、瞬心到车体中心的距离满足下式：
$r_c =\frac{ v_c}{w}(w\neq0)$
2、瞬心的角度与速度的朝向垂直
3、瞬心的位置与角速度的方向相关，当角速度为正时，瞬心的角度为速度方向逆时针旋转90度，当角速度为负时，瞬心的角度为速度方向顺时针旋转90度。
关于第二点的理解再于：当计算某一时刻的瞬心时，默认小车在这一时刻是一个完全的刚体，其按照一定的线速度角速度做圆周运动，则其圆周运动的半径为 $r_c$ 且垂直当前速度方向。
摘录一张示意图：
在这里插入图片描述
通过上述的约束，对于任意时刻当已知线速度与角速度的情况，我们可以计算出瞬心的坐标：
$\theta = arctan\frac{ v_x}{v_y}\pm\frac{ \pi}{2}$
$R_c =\frac{ v_c}{w}(w\neq0)$
$\begin{align} icc_x = R_c*sin\theta\\ icc_y = R_c*cos\theta \end{align}$
函数实现：

// 计算速度向量角
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$ )，瞬心的坐标为：( $ICR_x$ , $ICR_y$ )。以瞬心为起点，四个轮子的坐标为终点作向量，即可以通过向量法求出四条线的角度：

$\begin{align} \theta_A = arctan\frac{ ICR_y-A_y}{ICR_x-A_x}\\ \theta_B = arctan\frac{ ICR_y-B_y}{ICR_x-B_x}\\ \theta_C = arctan\frac{ ICR_y-C_y}{ICR_x-C_x}\\ \theta_D = arctan\frac{ ICR_y-D_y}{ICR_x-D_x} \end{align}$
然后，同样采用刚体的思想考虑每个轮子的状态，则在该时刻轮子的角度应与轮子到瞬心连线垂直，即 $\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轴上，但是总体还是能体现出瞬心与轮子的角度间的关系的。