移动机器人运动学

移动机器人运动学

控制对象的正逆解

  要控制一个机器人，首先得弄清楚输入与输出:
  例如，一个两轮差速机器人，我们的控制对象是左右轮的转速，输出的是机器人整体的线速度和角速度；对于一个机械臂，我们控制的是各个关节电机的角度（目前大部分电机），输出的是机械臂末端的空间坐标和空间角（六维）。
  一般来说，控制对象有n个变量，通常把这些变量叫做广义坐标，而输出有m个变量。
  广义坐标：
$${\mathbf{q}}_n=\left[q_1,q_2,...q_n\right]$$
  输出：
$${\mathbf{x}}_m=\left[x_1,x_2,...x_m\right]$$
  由 $q_n$计算 $x_m$就是正解，反过来是逆解。
$$\begin{gathered} {\mathbf{x}}_m=f\left({\mathbf{q}}_{\mathbf{n}}\right) \\ {\mathbf{q}}_n=f^{-1}\left({\mathbf{x}}_{\mathbf{m}}\right) \end{gathered}$$

齐次坐标变换

  1. 一个机器人身上存在的不同的传感器，比如激光雷达、IMU、摄像头等，它们都有自己的坐标系，比如不同位置的摄像头拍摄的画面，而在使用它们的时候，需要转化到一个坐标系下。
  2. 机器人在运动时也需要使用不同坐标系进行描述，机器人自身有一个本体坐标系，同时还有一个基于地图的世界坐标系，我们要求机器人到达某个位置，就是相对于世界坐标系来说的。
  3. 例如机械臂，每一个关节就是一个坐标系。

将一个点从一个坐标系变换到另一个坐标系，左乘变换矩阵：
$$p_A=T_B^A{p}_B$$
怎么找 $T_B^A$：

1. 固定A坐标系，考察绝对旋转，则旋转矩阵为左乘：
   $$R=R_z{R}_y{R}_x$$p为原点坐标做差
   $$T=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]$$
2. 考察相对旋转，旋转矩阵是右乘
   $$R=R_x{R}_y{R}_z$$

机器人运动学模型

  要得到机器人的速度：世界坐标下的速度->机器人坐标系下的速度->轮子的转速(->PWM大小)。在这个过程中，不涉及力的作用。

世界坐标下的速度->机器人坐标系下的速度

一般来说，机器人的线速度方向就是X_robot的方向，则易知：
$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\end{array}\right]=\left[\begin{array}{c}\cos \theta \\ \sin \theta \end{array}\right]v_x$$
$\theta$ 就是机器人的角度，没啥变换。所以结合起来可以得到：
$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{cc}\cos \theta & 0\\ \sin \theta & 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\\ \omega \end{array}\right]$$其中 $v$是机器人的线速度，$\omega$是机器人的角速度，$\theta$是机器人线速度在世界坐标系下的角度，这个方程对于所有机器人都适用。

机器人坐标系下的速度->轮子的转速

这个对不同机器人来说，方程就不同了，常见的机器人有如下几种。

1. 两轮差速机器人
     机器人在运动过程中同时具有平移和旋转运动，当机器人的旋转运动可以看成绕某点运动，则左右轮出的角速度应该相等，由此计算出机器人的角速度：
   $$\omega =\frac{v_l}{R}=\frac{v_r}{R+L}⟹\omega =\frac{v_r-v_l}{L}$$这里是不考虑轮子的滑动，一般来说机器人比较重，也不会有滑动。机器人的线速度为：
   $$v=\frac{v_l+v_r}{2}$$

2. 三轮车模型
   看图，A、B、C三个轮子均为全向轮，
   
   这个推导可以这么看：当机器人只有速度 $v_x$时，A轮无速度，C轮正向，B轮反向，把速度进行分解：$$v_x\cos 30°=v_c=-v_b$$同理，当机器人只有速度 $v_y$时，$$\begin{gathered} v_y=v_a \\ {v}_y\cos 60°=-v_c=-v_b \end{gathered}$$当机器人角速度为 $\omega$时$$\omega =\frac{v_a}{d}=\frac{v_b}{d}=\frac{v_c}{d}$$综合以上分析可以得到$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\omega }\end{array}\right]=\left[\begin{array}{ccc}0& \frac{1}{\sqrt{3}}& \frac{1}{\sqrt{3}}\\ \frac{1}{3}& -\frac{2}{3}& -\frac{2}{3}\\ \frac{1}{3d}& \frac{1}{3d}& \frac{1}{3d}\end{array}\right]\left[\begin{array}{c}{v}_a\\ v_b\\ v_c\end{array}\right]$$

3. 轮子的转速->PWM大小
     得到转速后，需要使转速保持稳定，因此需要对每个车轮都做控制，常用的就是PID控制，因为一般来说电机是通过PWM控制的，这个PWM的部分不好建模，因此现控中的控制方法不太方便，PID就简单直接方便。