Bicycle Kinematic Model 實作小筆記

Class 實作：

import numpy as np

class Bicycle():
    def __init__(self):
        self.xc = 0
        self.yc = 0
        self.theta = 0
        self.delta = 0
        self.beta = 0

        self.L = 2
        self.lr = 1.2
        self.w_max = 1.22

        self.sample_time = 0.01

    def reset(self):
        self.xc = 0
        self.yc = 0
        self.theta = 0
        self.delta = 0
        self.beta = 0

    def step(self, v, w):
        # ==================================
        #  Implement kinematic model here
        # ==================================
        if w > 0:
            w = min(w, self.w_max)
        else:
            w = max(w, -self.w_max)

        self.delta += (w * self.sample_time)
        self.beta = np.arctan(self.lr * np.tan(self.delta) / self.L)

        theta_dot = v * np.cos(self.beta) * np.tan(self.delta) / self.L
        self.theta += theta_dot * self.sample_time

        self.xc += (v * np.cos(self.theta + self.beta)) * self.sample_time 
        self.yc += (v * np.sin(self.theta + self.beta)) * self.sample_time

要怎麼畫出 8 - Concept：

要怎麼畫出 8 - 實作

sample_time = 0.01
time_end = 30
model.reset()

t_data = np.arange(0,time_end,sample_time)
x_data = np.zeros_like(t_data)
y_data = np.zeros_like(t_data)
v_data = np.zeros_like(t_data)
w_data = np.zeros_like(t_data)

# ==================================
#  Learner solution begins here
# ==================================
radius = 8
v_data = np.zeros_like(t_data)
v_data[:] = 2 * (2 * radius * np.pi) / time_end

w_data = np.zeros_like(t_data)
delta = 0.985 * np.arctan(model.L/radius)

for i in range(t_data.shape[0]):
    x_data[i] = model.xc
    y_data[i] = model.yc

    if i <= t_data.shape[0]/8:
        if model.delta < delta:
            model.step(v_data[i], model.w_max)
            w_data[i] = model.w_max
        else:
            model.step(v_data[i], 0)
            w_data[i] = 0

    elif i <= 5.05 * t_data.shape[0]/8:    
        if model.delta > -delta:
            model.step(v_data[i], -model.w_max)
            w_data[i] = -model.w_max
        else:
            model.step(v_data[i], 0)
            w_data[i] = 0

    else:
        if model.delta < delta:
            model.step(v_data[i], model.w_max)
            w_data[i] = model.w_max
        else:
            model.step(v_data[i], 0)
            w_data[i] = 0

    # print("step=%i, (x=%s, y=%s), (model=%s, delta=%s)" % (i, np.round(x_data[i], 4), np.round(y_data[i], 4), np.round(model.delta, 6), np.round(delta, 6)))

# ==================================
#  Learner solution ends here
# ==================================
plt.axis('equal')
plt.plot(x_data, y_data)
plt.show()