CarRacing 车辆模型 学习

任务

终于要自己改代码了，要做基于模型的控制，肯定得知道模型是怎么设计的。网上没有找到介绍gym car racing环境的详细介绍，打算自己扒一扒源码搞清楚。知道输入的控制量和状态之间的状态空间方程是啥样的就行。

探索过程

主要参考

git下gym的源码，发现和这个环境相关的主要是car_dynamics.py、car_racing.py两个脚本还有car_dynamics参考的项目http://www.iforce2d.net/b2dtut/top-down-car。虽然这个项目有代码的较详细解释，但毕竟也非常非常长，也不知道和gym的逻辑是否完全一样，就和代码一起参考着看吧。

car_racing.py的step函数

中文是我的注释，英文是原注释：

# 这个step是CarRacing类的
    def step(self, action: Union[np.ndarray, int]):
        assert self.car is not None
        if action is not None:
            # 只看contimuous的
            if self.continuous:
                # 使用了car_dynamics.py中定义的car类
                # 把转向动作取反，交给car类的steer函数
                self.car.steer(-action[0])
                # 把油门动作交给car类的gas函数
                self.car.gas(action[1])
                # 把刹车动作交给car类的brake函数
                self.car.brake(action[2])
            else:
                if not self.action_space.contains(action):
                    raise InvalidAction(
                        f"you passed the invalid action `{action}`. "
                        f"The supported action_space is `{self.action_space}`"
                    )
                self.car.steer(-0.6 * (action == 1) + 0.6 * (action == 2))
                self.car.gas(0.2 * (action == 3))
                self.car.brake(0.8 * (action == 4))

        # 更新汽车状态和环境状态
        # 计算每帧的时间1/FPS，交给car类的step函数
        self.car.step(1.0 / FPS)
        # 把每帧时间，和好像是空间大小信息交给world类的step函数
        self.world.Step(1.0 / FPS, 6 * 30, 2 * 30)
        # 一帧运行一次这个CarRacing类的step函数，并用t维护运行时间
        self.t += 1.0 / FPS

        self.state = self._render("state_pixels")

        step_reward = 0
        terminated = False
        truncated = False
        if action is not None:  # First step without action, called from reset()
            self.reward -= 0.1
            # We actually don't want to count fuel spent, we want car to be faster.
            # self.reward -=  10 * self.car.fuel_spent / ENGINE_POWER
            self.car.fuel_spent = 0.0
            step_reward = self.reward - self.prev_reward
            self.prev_reward = self.reward
            # 感觉应该是跑完所有赛道，就truncated
            if self.tile_visited_count == len(self.track) or self.new_lap:
                # Truncation due to finishing lap
                # This should not be treated as a failure
                # but like a timeout
                truncated = True
            # 获取汽车位置
            x, y = self.car.hull.position
            # 如果汽车超出了预设的范围，就terminated
            if abs(x) > PLAYFIELD or abs(y) > PLAYFIELD:
                terminated = True
                step_reward = -100

        if self.render_mode == "human":
            self.render()
        return self.state, step_reward, terminated, truncated, {}

大概意思是CarRacing兑现通过step函数接收到动作后，直接交给Car类对象的steer,gas,brake函数去计算状态的中间值，最后再用Car类对象的step函数做最终的状态更新，也用World类对象的step函数做环境的状态更新。如果跑完全程就truncated,如果偏离跑道太多就terminated。因为我暂时不用强化学习做，所以奖励怎么设计的我不关心。接下来就要去看car_dynamics.py脚本中steer,gas,brake函数的具体实现。

car_dynamics.py的steer,gas,brake函数

gas

油门逻辑很好理解，但是分段非线性的，有点烦人：

def gas(self, gas):
        """control: rear wheel drive

        Args:
            gas (float): How much gas gets applied. Gets clipped between 0 and 1.
        """
        # 油门限制在0，1范围内
        gas = np.clip(gas, 0, 1)
        # 只对后轮进行操作
        for w in self.wheels[2:4]:
            # 给每个后轮的油门赋值为输入的油门值
            # 如果要加速，但此次油门和上次油门差值不能大于0.1
            # 如果要减速，此次油门不受限制（逐渐加速，一下子减速。如果当前车轮油门是0.5，输入油门是1.0，需要5帧才能真的把1.0油门给到车轮，但如果油门是0，则车轮油门也直接清零）
            diff = gas - w.gas
            if diff > 0.1:
                diff = 0.1  # gradually increase, but stop immediately
            w.gas += diff

brake

刹车也很好理解，但又是一个分段非线性，气死：

def brake(self, b):
        """control: brake

        Args:
            b (0..1): Degree to which the brakes are applied. More than 0.9 blocks the wheels to zero rotation"""
        # 刹车对每个车轮都进行操作
        # 特别地，如果刹车值大于0.9，就完全锁死车轮
        for w in self.wheels:
            w.brake = b

这个非线性逻辑应该体现在后面对于w.brake的处理过程中，应该在step里

steer

转向也很好理解，就是给两个前轮赋值：

def steer(self, s):
        """control: steer

        Args:
            s (-1..1): target position, it takes time to rotate steering wheel from side-to-side"""
        # 转向只对前轮进行操作
        # 转向不能瞬时完成
        self.wheels[0].steer = s
        self.wheels[1].steer = s

对于wheel.steer的处理，应该也在step里。

总结：gas,brake,steer函数基本都属于仅仅给类变量赋值，具体怎么用这个值更新汽车状态应该在step函数里，接下来看step函数。

car_dynamics.py的step函数

看了step函数才知道这个有多麻烦。。。我放弃了，不用这个复杂的模型了，继续用之前git项目的简单模型，反正我就是做个课程大作业而已，而且我的工作重点也不在这里，快过年了，赶紧把重点部分搞定，这个模型以后有机会再看吧。先把看过的东西放这里：

def step(self, dt):
        for w in self.wheels:
            # Steer each wheel
            # 获得转向方向
            dir = np.sign(w.steer - w.joint.angle)
            # 获得转向角度
            val = abs(w.steer - w.joint.angle)
            # 方向 * 角度 得到转向电机速度，限定最小转速是3
            w.joint.motorSpeed = dir * min(50.0 * val, 3.0)

            # Position => friction_limit
            # 计算最大摩擦力
            # 先计算草地的摩擦力
            grass = True
            # 如果不是赛道，是草地，他的摩擦力极限就是 0.6（草地摩擦系数） * FRICTION_LIMIT
            friction_limit = FRICTION_LIMIT * 0.6  # Grass friction if no tile
            # 再计算道路瓦片的摩擦力
            for tile in w.tiles:
                # 每个瓦片的摩擦力都是 FRICTION_LIMIT * 瓦片摩擦系数
                # 遍历所有瓦片，取最大的摩擦力
                friction_limit = max(
                    friction_limit, FRICTION_LIMIT * tile.road_friction
                )
                grass = False

            # Force
            # GetWorldVector是获取局部坐标系中的向量在全局坐标系中的投影
            # 获得轮胎坐标系y轴在世界坐标系的x轴和y轴上的投影
            # forw0是 轮胎y轴也就是侧向方向 在 世界坐标系x轴上的投影； forw1是 轮胎y轴也就是侧向方向 在 世界坐标系y轴上的投影
            forw = w.GetWorldVector((0, 1))
            # 获得轮胎坐标系x轴在世界坐标系的x轴和y轴上的投影
            # side0是 轮胎x轴也就是侧向方向 在 世界坐标系x轴上的投影； side1是 轮胎x轴也就是侧向方向 在 世界坐标系y轴上的投影
            side = w.GetWorldVector((1, 0))
            # 获得轮胎速度，在轮胎坐标系下的值，V0是轮胎的X方向也就是前向速度，V1是轮胎的Y方向也就是侧向速度
            v = w.linearVelocity

            vf = forw[0] * v[0] + forw[1] * v[1]  # forward speed
            vs = side[0] * v[0] + side[1] * v[1]  # side speed

            # WHEEL_MOMENT_OF_INERTIA*np.square(w.omega)/2 = E -- energy
            # WHEEL_MOMENT_OF_INERTIA*w.omega * domega/dt = dE/dt = W -- power
            # domega = dt*W/WHEEL_MOMENT_OF_INERTIA/w.omega
            # 动能E = 惯量 * （角速度*角速度）/2
            # 功率P = 动能的微分 = E/dt = 惯量 * 角速度 * 角速度/dt
            # 进而角速度 = dt * 功率P / 惯量 / 角速度

            # add small coef not to divide by zero
            # 车轮的转速使用上述的方程计算的： 角速度 = dt * 功率P / 惯量 / 角速度
            # 不过功率P是 初始功率P * 油门
            # 上一时刻角速度 是 上一时刻角速度 + 一个小量（5）
            w.omega += (
                dt
                * ENGINE_POWER
                * w.gas
                / WHEEL_MOMENT_OF_INERTIA
                / (abs(w.omega) + 5.0)
            )
            # 我们先不考虑能耗
            self.fuel_spent += dt * ENGINE_POWER * w.gas

            # 如果刹车》0.9 车轮转速直接一下干到0
            if w.brake >= 0.9:
                w.omega = 0
            # 如果没到0.9，就用刹车×最大刹车力，用车轮转速-刹车力，最小刹到0速
            elif w.brake > 0:
                BRAKE_FORCE = 15  # radians per second
                dir = -np.sign(w.omega)
                val = BRAKE_FORCE * w.brake
                if abs(val) > abs(w.omega):
                    val = abs(w.omega)  # low speed => same as = 0
                w.omega += dir * val
            w.phase += w.omega * dt

            # 角速度*半径得到线速度
            vr = w.omega * w.wheel_rad  # rotating wheel speed
            # 车辆前向速度 = 前向摩擦力 + 车轮转速
            f_force = -vf + vr  # force direction is direction of speed difference
            # 车辆侧向速度 = 侧向摩擦力
            p_force = -vs

            # Physically correct is to always apply friction_limit until speed is equal.
            # But dt is finite, that will lead to oscillations if difference is already near zero.

            # Random coefficient to cut oscillations in few steps (have no effect on friction_limit)
            f_force *= 205000 * SIZE * SIZE
            p_force *= 205000 * SIZE * SIZE
            force = np.sqrt(np.square(f_force) + np.square(p_force))

            # Skid trace
            # 如果力超过两倍摩擦力，就可能打滑
            if abs(force) > 2.0 * friction_limit:
                if (
                    w.skid_particle
                    and w.skid_particle.grass == grass
                    and len(w.skid_particle.poly) < 30
                ):
                    w.skid_particle.poly.append((w.position[0], w.position[1]))
                elif w.skid_start is None:
                    w.skid_start = w.position
                else:
                    w.skid_particle = self._create_particle(
                        w.skid_start, w.position, grass
                    )
                    w.skid_start = None
            else:
                w.skid_start = None
                w.skid_particle = None

            if abs(force) > friction_limit:
                f_force /= force
                p_force /= force
                force = friction_limit  # Correct physics here
                f_force *= force
                p_force *= force

            w.omega -= dt * f_force * w.wheel_rad / WHEEL_MOMENT_OF_INERTIA

            w.ApplyForceToCenter(
                (
                    p_force * side[0] + f_force * forw[0],
                    p_force * side[1] + f_force * forw[1],
                ),
                True,
            )

大概就是计算车体坐标系和世界坐标系的相对关系，计算得到车的前向力和侧向力，然后转移到世界坐标系，然后交给BOX2D库做动力学（所以还得看BOX2D库的实现，这也太麻烦了）。
前向力由摩擦造成的整体速度和每个车轮速度决定；侧向速度由侧向摩擦力决定。
摩擦力由地面材质、方向、速度决定。
太麻烦了，阿巴阿巴。先这样了。如果有哪位看到本文的大哥把程序解读好了，麻烦也给我分享下，或者自己写了一个简单点的step，也共享一下，thanks。