Webots中获取机器人质心三维速度

Webots中获取机器人质心三维速度

---

0 前言

  Webots中没法直接获取机身坐标系下的三维质心速度，而很多算法都需要机身坐标系下的三维质心速度比如虚拟模型控制（VMC）算法、MIT开源的模型预测控制算法等。本文参考谷歌团队开源MPC控制算法motion_imitation中求取机身质心速度的方法，核心部分代码如下面代码块。这是在Pybullet中实现的，相关的invertTransform和multiplyTransforms函数的介绍参考我的另一篇文章Pybullet矩阵变换相关API。

  def update(self, current_time):
    del current_time
    velocity = self._robot.GetBaseVelocity()

    vx = self._velocity_filter_x.calculate_average(velocity[0])
    vy = self._velocity_filter_y.calculate_average(velocity[1])
    vz = self._velocity_filter_z.calculate_average(velocity[2])
    self._com_velocity_world_frame = np.array((vx, vy, vz))

    base_orientation = self._robot.GetTrueBaseOrientation()
    _, inverse_rotation = self._robot.pybullet_client.invertTransform(
        (0, 0, 0), base_orientation)

    self._com_velocity_body_frame, _ = (
        self._robot.pybullet_client.multiplyTransforms(
            (0, 0, 0), inverse_rotation, self._com_velocity_world_frame,
            (0, 0, 0, 1)))

  其本质上就是利用旋转矩阵直接将速度矢量进行旋转变换，从而将质心全局速度变换到机身坐标系下表示，后面将讲述如何在Webots中实现，以及原理。本人将这个方法应用到四足机器人中的虚拟模型控制中求取速度，效果不错，部分结果展示如下：

• 上坡：
  
• 下坡：
  
• 转弯：
  
• 推搡：
  
• 多次侧向推搡，机身侧向速度曲线图如下，可以稳定：
  

1 旋转矩阵

  假设全局坐标系为$s$，机身坐标系为$b$，我们根据Webots中的IMU可以轻易求得从坐标系$s$变换到坐标系$b$旋转矩阵为$R_s^b(t)$，具体怎么求马上讲，别急。
  求得旋转矩阵后，我们需要利用GPS做差分，得到机身速度在全局坐标系下的表示记为$v_s$（Webots没有像Pybullet一样能直接获取刚体速度的函数），同理机身速度在机身坐标系下的表示为$v_b$，那么可以很轻易的得到他们的关系如下：
$$v_s=R_s^b(t)v_b$$
  亦即:
$$v_b=R_s^b(t{)}^{-1}{v}_s=R_s^b(t{)}^T{v}_s$$
  注意速度大小不随表示坐标系而改变，这是由旋转矩阵的性质决定的，保长度。

1.1 Webots中旋转矩阵的求取

  首先我们看官方手册对IMU的描述：

  废话有点多，重点就是：

• 按手册所述配置好IMU坐标轴方向
• IMU旋转顺序是YZX，即先绕X轴旋转，再绕Z轴旋转，最后绕Y轴旋转
    我们知道了旋转顺序就好说了，由于欧拉角是绕固定坐标系旋转，所以左乘，乘积如下：

$$R_s^b=Rot(y,\beta )\cdot Rot(z,\gamma )\cdot Rot(x,\alpha )$$

  其中$\alpha$为横滚角roll，$\gamma$为俯仰角pitch，$\beta$为偏航角yaw。很容易根据旋转矩阵的公式求得：
$$R_s^b=\left[\begin{array}{ccc}{c}_{\alpha }{c}_{\gamma }& -c_{\beta }{c}_{\alpha }{s}_{\gamma }+s_{\alpha }{s}_{\beta }& c_{\beta }{s}_{\alpha }{s}_{\gamma }+c_{\alpha }{s}_{\beta }\\ s_{\gamma }& c_{\gamma }{c}_{\alpha }& -c_{\gamma }{s}_{\alpha }\\ -s_{\beta }{c}_{\gamma }& s_{\beta }{c}_{\alpha }{s}_{\gamma }+s_{\alpha }{s}_{\beta }& -s_{\beta }{s}_{\alpha }{s}_{\gamma }+c_{\alpha }{c}_{\beta }\end{array}\right]$$
  其中$s$代表$sin$，$c$代表$cos$，加个下标分别代表求对应角的正余弦值。有了这个东东后面就好办了，我弄了一个小车进行了测试，下面见代码。

2 代码展示

  小车IMU坐标系和全局坐标系如下图所示：

  我们可以测试我们刚刚求取的旋转矩阵是否正确，如下图所示：

  由此可见旋转矩阵求取正确，机身$x$轴在全局坐标系下的表示为$[001{]}^T$，其他两轴也都正确。
  下面我们测试我们得到的机身坐标系下的速度和全局坐标系下的速度是否相同，相同岂不是我们白做工作了。利用坡道测试，当小车上坡时，全局坐标系下的$y$方向和$z$方向速度分量肯定不小，但机身坐标系下$y$和z方向速度肯定很小。结果如下：


  其中由GPS求出的全局坐标系下速度的幅值为0.15左右，机身坐标系下的速度（local speed）只有x方向有较大速度，而全局坐标系下的速度表示在$y$方向也有一个不可忽略的值。
  代码如下，实际上你可以对这个求GPS微分的过程进行一个滤波处理，防止扰动带来高频噪声，你也可以把小车悬空，测试小车下降速度，这里就不展示了。

2.1 代码说明与展示

  函数com_velocity_estimate()返回三个值，可以打印返回的旋转矩阵，看看IMU坐标轴在全局坐标轴的位置，是不是正确的以验证旋转矩阵求取的正确性，最后用求得的速度和GPS自带的一维速度获取函数进行比较，以及与直接微分求得的全局速度进行比较，发现误差还是不大的。

from controller import Robot
import numpy as np
import math
import copy

robot = Robot()
timeStep = int(robot.getBasicTimeStep())

motor = [robot.createMotor('LF'), robot.createMotor('RF'), robot.createMotor('RH'), robot.createMotor('LH')]

GPS = robot.createGPS("gps")
IMU = robot.createInertialUnit("IMU")

IMU.enable(timeStep)
GPS.enable(timeStep)

for i in range(4):
    motor[i].setPosition(float('+inf'))
    motor[i].setVelocity(-3)

pre_pos = np.array(GPS.getValues())


def com_velocity_estimate():
    global pre_pos

    euler = IMU.getRollPitchYaw()

    s_r = math.sin(euler[0])
    c_r = math.cos(euler[0])
    s_p = math.sin(euler[1])
    c_p = math.cos(euler[1])
    s_y = math.sin(euler[2])
    c_y = math.cos(euler[2])

    pos = np.array(GPS.getValues())

    velocity = (pos - pre_pos) / (0.001 * timeStep)

    pre_pos = copy.deepcopy(pos)
    rotate = np.zeros((3, 3), dtype=float)
    rotate[0, 0] = c_y * c_p
    rotate[0, 1] = s_r * s_y - c_y * c_r * s_p
    rotate[0, 2] = c_y * s_r * s_p + s_y * c_r
    rotate[1, 0] = s_p
    rotate[1, 1] = c_p * c_r
    rotate[1, 2] = -c_p * s_r
    rotate[2, 0] = -s_y * c_p
    rotate[2, 1] = c_r * s_y * s_p + s_r * c_y
    rotate[2, 2] = -s_y * s_r * s_p + c_r * c_y

    return rotate.T @ velocity, rotate, velocity


while robot.step(timeStep) != -1:
    # Read the sensors:
    # Enter here functions to read sensor data, like:
    #  val = ds.getValue()
    print("gps speed:{0}".format(GPS.getSpeed()))
    a, b, c = com_velocity_estimate()
    # print("gps_pos:{0},{1},{2}".format(gps_pos[0], gps_pos[1], gps_pos[2]))
    # print("acc:{0},{1},{2}".format(acc[0], acc[1], acc[2]))
    # print("euler:{0},{1},{2}".format(euler[0], euler[1], euler[2]))
    # print(b)
    print("global speed:{0}".format(c))
    print("local speed:{0}".format(a))

小车模型和整个项目代码百度网盘自取：
链接：https://pan.baidu.com/s/1Kw4f4k0Rorj5N09dUqL83A
提取码：znb5

3 基于Supervisor控制器获取机器人速度

  这部分是我在2021.10.04更新的，看了Webots2021b的更新日志之后发现，Webots2021b已经支持直接获取节点的速度与角速度（注意Webots2021b之前的版本是不支持的），其次单击节点，左下角的Field Edit中能看到机器人的实时速度与角速度，还可以选择相对机器人的速度，如下图所示：

  但是目前阅读了Webots2021b相关源码后发现，只有Supervisor控制器才有获取速度的API，也就是如下图所示必须在机器人节点下勾选supervisor为true，然后参考Webots中自带的supervisor控制器例程，去写机器人控制器。

  可以见下图（Python），通过DEF或其他API获取需要的节点，然后就可以通过getVelocity函数获取该节点的速度，但只能获取全局坐标系下的绝对速度，函数描述见下下图：


PS:Webots2021b的素材安装时不会自动下载，得联网去Github下载，国内DNS污染，很可能链接不了Github去下载素材，没有科学上网工具不要安装。