问题的描述是这样的(用我理解后的语言来表达):

以搭载2D激光雷达(2D-Lidar)的扫地机器人为例,只考虑在二维平面下的关系。世界坐标系$O_W$,机器人坐标系$O_C$,雷达坐标系$O_L$,雷达坐标系的方向轴与机器人坐标系一致,雷达搭载在机器人上,用机器人坐标系$O_C$的坐标表达,雷达的位置为$(xlc, ylc)$,现在空间中有一个物体点P,通过2D雷达对其测量(雷达旋转方向为逆时针),得到对应的角度$angle$和距离$dis$,现在假设扫地机器人运动的角速度是$w_c$,线速度是$v_c$,在$t_0$时刻时,机器人坐标系$O_C$与世界坐标系$O_W$重合,这里的角度单位都是弧度,距离单位都是米。

现在给这样一组数据,问在$t_1$时刻时,物体点P在世界坐标系$O_W$下的坐标:

angle、dis、xlc、ylc、 vc、wc、t1

需要理解的前提

首先,对于输入数据的理解:可以将测试数据作为一个验证情况,因为,当世界坐标系$O_W$,机器人坐标系$O_C$,雷达坐标系$O_L$之间的关系表达明确时,不管带入什么测试数据,都应该是满足的。

另外,有一个常识需要补充:在C++环境以及处理物体学、数学分析等进行三角函数计算时,使用的都是弧度制, 1弧度 $\frac{180}{\pi}$度,1 度 = $\frac{\pi}{180}$弧度。

double degrees = 90.0;
double radians = degrees * (M_PI / 180.0);
double sinValue = sin(radians);  // 使用弧度

🌟进行一步一步拆解

t时间后机器人运动的位置

首先需要理解,扫地机器人是怎么运动的,在这里知道机器人运动同时拥有角速度$w_c$,线速度$v_c$。

一般情况下,移动机器人是通过控制两个并行轮子的速度差(差速)进行旋转的。两个轮子速度相同时,移动机器人将直线运动,两个轮子速度不同时,移动机器人将绕着某一点进行旋转运动,这个点就是即时转动中心(Instantaneous Center of Rotation, 简称 ICC)。

也就是说,机器人此时的运动模型是一个圆弧运动,运动的半径$R = \frac{v_c}{w_c}$,运动的角度可以通过$\theta = w \cdot t$得到,而对于机器人中心点的位置情况则可以通过极坐标的关系来得到,也就是机器人中心为一点,绕着ICC为原点,进行半径为R的圆周运动,转动了$\theta = w \cdot t$角度,那么转动$t$时间后机器人中心点的位置以XY坐标系(笛卡尔坐标系)表达就是:

  • $X_{t} = R \sin(w_c \cdot t)$
  • $Y_{t} = R (1 - \cos(w_c \cdot t))$

由此假设以机器人坐标系$O_C$原点来计算,那么在t时间后,$O_C$原点的坐标在世界坐标系$O_W$上的表达为:

  • $X_{W_C} = R \sin(w_c \cdot t)$
  • $Y_{W_C} = R (1 - \cos(w_c \cdot t))$

即时转动中心(Instantaneous Center of Rotation)

在任意给定瞬间,一个在平面上进行复合运动(即同时包含平移和旋转)的物体看起来是围绕一个假想点进行旋转,这个点就是即时转动中心。

这个点可能位于物体内部,也可能位于物体外部(取决于左右轮的速度),甚至可能无限远(这种情况下,物体的运动可以看作是纯平移)。

假设一个通过两个并行轮子进行差速驱动的移动机器人,左右轮子的速度分别为$V_l$和$V_r$,两轮中心之间距离为$L$,那么可以定义机器人的线速度$v_c$和角速度$w_c$如下:

  • 线速度$v_c$,表示机器人移动时机器人中心点的速度,通过左右轮速度的平均值来估计: $$ v_c = \frac{V_r + V_l}{2} $$
  • 角速度$w_c$描述了机器人绕ICC的旋转速度,可以通过左右轮速度差与轮距的比值来计算(通过$v = \omega \cdot r$得到): $$ w_c = \frac{V_r - V_l}{L} $$

机器人绕ICC的圆弧运动半径$R$(从ICC到机器人中心的距离),根据$v = \omega \cdot r$可以得到: $$ R = \frac{v_c}{w_c} $$

将上述定义的$v_c$和$w_c$代入到$R$的公式中可以得到: $$ R = \frac{\frac{V_r + V_l}{2}}{\frac{V_r - V_l}{L}} $$ $$ R = \frac{L}{2} \cdot \frac{V_r + V_l}{V_r - V_l} $$

另一种数学解释

对于ICC的计算,在任意瞬间,机器人的左右轮到ICC的距离固不变,形成一个圆周运动。假设右轮比左轮快,那么ICC位于机器人左侧,ICC距离机器人的中心点为$R$。

由于机器人的左右两个轮子绕ICC的旋转半径不同,但是它们完成一次完整旋转所需的时间相同,因此有:

  • 左轮的圆周速度是$V_l$,旋转半径是$R - \frac{L}{2}$。
  • 右轮的圆周速度是$V_r$,旋转半径是$R + \frac{L}{2}$。

根据圆周运动的速度公式$v = \omega \cdot r$,得到:

  • $V_l = w_c \cdot (R - \frac{L}{2})$
  • $V_r = w_c \cdot (R + \frac{L}{2})$

将$w_c$带入解方程可以得到:

$$ R = \frac{L}{2} \cdot \frac{V_r + V_l}{V_r - V_l} $$

但是更直观的方式是利用角速度和线速度的定义来直接求解$R$,如下:

$$ w_c = \frac{V_r - V_l}{L} \Rightarrow w_c \cdot L = V_r - V_l $$

并且

$$ v_c = \frac{V_r + V_l}{2} $$

而$R$可以理解为机器人(中心点)绕ICC旋转的半径,其线速度$v_c$与角速度$w_c$之间存在如下关系:

$$ R = \frac{v_c}{w_c} $$

圆周运动中的位置计算

对于绕ICC的圆周运动,可以用极坐标系下的圆方程来描述物体的位置。在极坐标系中,一个点的位置由它到原点的距离(半径$R$)和一个角度($\theta$)来定义。

将ICC视为原点,则机器人中心就是这个点,其位置转换为笛卡尔坐标系来表示就是:

  • $X = R \sin(\theta)$
  • $Y = R - R \cos(\theta) = R (1 - \cos(\theta))$

物体点P在机器人坐标系$O_C$的表达

将2D-Lidar看作为一个点,也就是原点$O_L$,那么在雷达坐标系$O_L$下,物体点P可以直接通过三角关系得到在雷达坐标系$O_L$的表达:

  • $X_{L_p} = dis \cdot \cos(angle)$
  • $Y_{L_p} = dis \cdot \sin(angle)$

而雷达坐标系$O_L$和机器人坐标系$O_C$只相差了一个平移关系,也就是$(xlc, ylc)$,因此可以将平移的量加上得到在机器人坐标系$O_C$的表达

  • $X_{C_p} = dis \cdot \cos(angle) + xlc$
  • $Y_{C_p} = dis \cdot \sin(angle) + ylc$

物体点P在世界坐标系$O_W$的表达

因为机器人坐标系$O_C$和世界坐标系$O_W$相差了一个旋转,机器人坐标系$O_C$从与世界坐标系$O_W$重合的状态,通过逆时针旋转$\theta = w \cdot t$得到现在的状态。

也就是说,以机器人坐标系$O_C$的表达的物体点P也需要通过这个**逆时针旋转$\theta = w \cdot t$**得到在当世界坐标系$O_W$的表达;

因此世界坐标系$O_W$的物体点P为$(X_{W_p},Y_{W_p})$可以通过$(X_{C_p},Y_{C_p})$应用上逆时针旋转,再加上$O_C$的偏移得到。

  • $X_{W_p} = X_{C_p} \cdot \cos{\theta} - Y_{C_p} \cdot \sin{\theta} + X_{W_C}$

  • $X_{W_p} = X_{C_p} \cdot \sin{\theta} + Y_{C_p} \cdot \cos{\theta} + Y_{W_C}$

注意,逆时针为正(以右手坐标系来看,X轴朝左,Y轴朝上)


逆时针旋转角度 $\theta$ 的旋转矩阵的证明

从二维旋转的基本几何出发。假设我们有一个点 $P$,在原点 $O$ 的坐标系中的初始位置为 $P(x, y)$,需要求出 $P$ 绕原点逆时针旋转角度 $\theta$ 后的新位置 $P’(x’, y’)$:

旋转前,点 $P$ 的位置可以由极坐标 $r$(原点到 $P$ 的距离)和 $\phi$($x$ 轴到 $OP$ 的角度)确定。在笛卡尔坐标系中,有:

  • $x = r\cos(\phi)$
  • $y = r\sin(\phi)$

当 $P$ 绕原点逆时针旋转 $\theta$ 后,它的新位置 $P’$ 可以用新的极坐标 $r$ 和新的角度 $\phi + \theta$ 来表示($r$ 保持不变,因为旋转不改变原点到 $P$ 的距离)。因此,旋转后的坐标为:

  • $x’ = r\cos(\phi + \theta)$
  • $y’ = r\sin(\phi + \theta)$

使用三角恒等式 $\cos(a + b) = \cos(a)\cos(b) - \sin(a)\sin(b)$ 和 $\sin(a + b) = \sin(a)\cos(b) + \cos(a)\sin(b)$,我们可以将 $x’$ 和 $y’$ 重写为:

  • $x’ = r\cos(\phi)\cos(\theta) - r\sin(\phi)\sin(\theta)$
  • $y’ = r\sin(\phi)\cos(\theta) + r\cos(\phi)\sin(\theta)$

将 $x = r\cos(\phi)$ 和 $y = r\sin(\phi)$,代入得到:

  • $x’ = x\cos(\theta) - y\sin(\theta)$
  • $y’ = x\sin(\theta) + y\cos(\theta)$

将上述方程写成矩阵形式,得到:

$$ \begin{bmatrix} x’ \ y’ \end{bmatrix} = \begin{bmatrix} \cos(\theta) & -\sin(\theta) \\ \sin(\theta) & \cos(\theta) \end{bmatrix} \begin{bmatrix} x \ y \end{bmatrix} $$ 这个矩阵就是逆时针旋转角度 $\theta$ 的旋转矩阵 $R(\theta)$,能够将任何给定的点在二维平面上逆时针旋转 $\theta$ 角度,而不改变点到原点的距离。

如果旋转是顺时针方向的,角度 $\theta$ 将被视为负值,以右手坐标系来看,X轴朝左,Y轴朝上时,逆时针旋转是正方向,顺时针旋转则是负方向。

顺时针旋转角度 $\theta$ 的旋转矩阵可以通过将逆时针旋转矩阵中的角度 $\theta$ 替换为它的负值 $-\theta$ 来得到,因为 $\cos(-\theta) = \cos(\theta)$ 和 $\sin(-\theta) = -\sin(\theta)$。这意味着,对于顺时针旋转,旋转矩阵 $R_{cw}(\theta)$ 是:

$$ R_{cw}(\theta) = \begin{bmatrix} \cos(-\theta) & -\sin(-\theta) \\ \sin(-\theta) & \cos(-\theta) \end{bmatrix} $$ 使用三角函数的性质 $\cos(-\theta) = \cos(\theta)$ 和 $\sin(-\theta) = -\sin(\theta)$,得到:

$$ R_{cw}(\theta) = \begin{bmatrix} \cos(\theta) & \sin(\theta) \\ -\sin(\theta) & \cos(\theta) \end{bmatrix} $$ 因此,顺时针旋转矩阵直接使用正角度 $\theta$ 为:

$$ R_{cw}(\theta) = \begin{bmatrix} \cos(\theta) & \sin(\theta) \\ -\sin(\theta) & \cos(\theta) \end{bmatrix} $$ 这个矩阵可以用来将任何给定的点在二维平面上顺时针旋转 $\theta$ 角度,这与逆时针旋转从形式上看只是$\sin(\theta)$的正负号相反。


完整的C++实现

#include <iostream>
#include <cmath>

int main() {
    // jj
    double angle, dis, xlc, ylc, vc, wc, t1;
    std::cin >> angle >> dis >> xlc >> ylc >> vc >> wc >> t1;
    // angle为弧度制,wc为弧度/s
    // dis为距离,vc为m/s

    // 起点(0, 0), 角速度wc, 线速度vc
    // 经过t1时间
    double delta_v = vc * t1;
    double delta_w = wc * t1;
    // 机器人坐标系Oc,世界坐标Ow
    double Xwc, Ywc;
    double R = vc / wc;
    Xwc = R * sin(delta_w);
    Ywc = R * (1 - cos(delta_w));
    // 打印Xwc, Ywc
    // std::cout << Xwc << ' ' << Ywc << std::endl; 

    // 求P点在世界坐标系Ow的坐标
    // 先求P点在Oc的坐标
    double Xcp, Ycp;
    Xcp = xlc + dis * cos(angle);
    Ycp = ylc + dis * sin(angle);
    // 打印Xcp, Ycp
    // std::cout << Xcp << ' ' << Ycp << std::endl;
    // 再求P点在Ow的坐标
    double Xwp, Ywp;
    Xwp = cos(delta_w) * Xcp - sin(delta_w) * Ycp + Xwc;
    Ywp = sin(delta_w) * Xcp + cos(delta_w) * Ycp + Ywc;
    // 打印Xwp, Ywp
    std::cout << Xwp << ' ' << Ywp << std::endl;

    return 0;
}