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

创作时间:

作者:

@小白创作中心

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

引用

CSDN

https://blog.csdn.net/YiYeZhiNian/article/details/142679555

四舵轮车辆在同时存在线速度与角速度时，其每个舵轮的角度计算是一个重要的控制问题。本文详细介绍了瞬心坐标的计算方法、舵轮角度的计算原理，并通过代码实现和测试结果展示了完整的解决方案。

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

1、瞬心的坐标计算

首先在瞬心的学习中可以知道对于四舵轮车辆，它的瞬心位置是任意的，但是满足以下条件：

瞬心到车体中心的距离满足下式：
r c = v c w ( w ≠ 0 ) r_c =\frac{ v_c}{w}(w\neq0)rc =wvc (w=0)
瞬心的角度与速度的朝向垂直
瞬心的位置与角速度的方向相关，当角速度为正时，瞬心的角度为速度方向逆时针旋转90度，当角速度为负时，瞬心的角度为速度方向顺时针旋转90度。

关于第二点的理解再于：当计算某一时刻的瞬心时，默认小车在这一时刻是一个完全的刚体，其按照一定的线速度角速度做圆周运动，则其圆周运动的半径为r c r_crc 且垂直当前速度方向。

摘录一张示意图：

通过上述的约束，对于任意时刻当已知线速度与角速度的情况，我们可以计算出瞬心的坐标：

θ = a r c t a n v x v y ± π 2 \theta = arctan\frac{ v_x}{v_y}\pm\frac{ \pi}{2}θ=arctanvy vx ±2π

R c = v c w ( w ≠ 0 ) R_c =\frac{ v_c}{w}(w\neq0)Rc =wvc (w=0)

i c c x = R c ∗ s i n θ i c c y = R c ∗ c o s θ \begin{align} icc_x = R_csin\theta\ icc_y = R_ccos\theta \end{align}iccx =Rc ∗sinθiccy =Rc ∗cosθ

函数实现：


// 计算速度向量角
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_xAx ,A y A_yAy )、(B x B_xBx ,B y B_yBy )、(C x C_xCx ,C y C_yCy )、(D x D_xDx ,D y D_yDy )，瞬心的坐标为：(I C R x ICR_xICRx ,I C R y ICR_yICRy )。以瞬心为起点，四个轮子的坐标为终点作向量，即可以通过向量法求出四条线的角度：

θ A = a r c t a n I C R y − A y I C R x − A x θ B = a r c t a n I C R y − B y I C R x − B x θ C = a r c t a n I C R y − C y I C R x − C x θ D = a r c t a n I C R y − D y I C R x − D x \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}θA =arctanICRx −Ax ICRy −Ay θB =arctanICRx −Bx ICRy −By θC =arctanICRx −Cx ICRy −Cy θD =arctanICRx −Dx ICRy −Dy

然后，同样采用刚体的思想考虑每个轮子的状态，则在该时刻轮子的角度应与轮子到瞬心连线垂直，即± π 2 \pm\frac{ \pi}{2}±2π 。取正取负仍然是由角速度决定，当角速度为逆时针时，则角度值应当加π 2 \frac{ \pi}{2}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;
}