無人車系統（三）：用python寫一個簡單的無人車仿真環境

原創

2020-02-21 19:13

仿真環境對於機器人研究來說太過重要，一個好的仿真環境能夠逼真的模擬真實的場景、擁有高保真的物理引擎，能夠比較真實的反映算法的性能。但是有時，我們有一個Idea，想馬上實現，從而快速的驗證這個idea的可行性，那麼一款小巧、簡潔的仿真環境可能更加合適。在得到驗證有那麼點意思後，再利用較真實的仿真環境驗證，進一步，再放在實際環境中進行測試。一般來說，簡單仿真環境——高保真仿真環境——真實環境，同一算法所需的代碼量是遞增的(因爲要考慮的外圍因素會增多，再加上接口代碼)。

簡單無人車仿真環境python代碼

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
%matplotlib inline

# set up matplotlib
is_ipython = 'inline' in matplotlib.get_backend()
if is_ipython:
    from IPython import display

plt.ion()
plt.figure(figsize=(18, 3))

class UGV_model:
    def __init__(self, x0, y0, theta0, L, v0, T): # L:wheel base
        self.x = x0 # X
        self.y = y0 # Y
        self.theta = theta0 # headding
        self.l = L  # wheel base
        self.v = v0  # speed
        self.dt = T  # decision time periodic
    def update(self, vt, deltat):  # update ugv's state
        dx = self.v*np.cos(self.theta)
        dy = self.v*np.sin(self.theta)
        dtheta = self.v*np.tan(deltat)/self.l
        self.x += dx*self.dt
        self.y += dy*self.dt
        self.theta += dtheta*self.dt
        
    def plot_duration(self):
        plt.scatter(self.x, self.y, color='r')   
        plt.axis([0, 18, -3, 3])
        if is_ipython:
            display.clear_output(wait=True)
            display.display(plt.gcf())  

# set reference trajectory
refer_path = np.zeros((100, 2))
refer_path[:,0] = np.linspace(0, 18, 100)


plt.plot(refer_path[:,0], refer_path[:,1], '-.b', linewidth=5.0)
ugv = UGV_model(0, 0, 0, 2.86, 2.0, 0.1)
for i in range(1000):
    ugv.update(2.0, np.cos(i/5.0))
    ugv.plot_duration()