使用python绘制阿克曼小车运动动画

 效果图：

python脚本： 

import struct
import threading
import time
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import matplotlib.patches as patches
import matplotlib.lines as lines
from math import sin,cos,tan,pi
import numpy as np

# 创建初始图形
fig, ax = plt.subplots(figsize=(6, 6))
def rotate_translate(points, angle, translation):
    # 构建旋转矩阵
    rotation_matrix = np.array([[np.cos(angle), -np.sin(angle)],
                                [np.sin(angle), np.cos(angle)]])

    # 平移向量
    translation_vector = np.array(translation)

    # 转换顶点列表为正确的形状
    vertices = np.array(points).reshape(-1, 2)

    # 进行旋转和平移
    transformed_vertices = np.dot(rotation_matrix, vertices.T).T + translation_vector

    # 将转换后的顶点列表转换为元组形式
    transformed_points = [tuple(vertex) for vertex in transformed_vertices]

    return transformed_points
class AkmanCar:
    def __init__(self,ax,init_pos) -> None:
        self.ax = ax
        self.init_pos = init_pos  #初始化位置
        self.pos = [0,init_pos]   #车辆位置
        self.Len = 3              #车长
        self.Width = 2            #车宽
        self.angle_wheel = 0      #前轮转角
        self.yaw = 0              #车身航向角
        self.yaw_w = 0            #车身航向角速度
        self.vy = 0               #车身前后方向速度

        self.body_points = [(-0.5*self.Width  , 0            ),(-0.5*self.Width  , self.Len     ),
                            ( 0.5*self.Width  , self.Len     ),( 0.5*self.Width  , 0            )]        #车身多边形顶点
        self.body = patches.Polygon(self.body_points, closed=True,lw = 1, edgecolor='k', facecolor='none')#车身多边形对象

        self.wheel_shape = [(-0.15,-0.3),(-0.15,0.3),(0.15,0.3),(0.15,-0.3)]                                 #轮子多边形顶点
        
        self.wheel_FL = patches.Polygon(self.wheel_shape, closed=True,lw = 1, edgecolor='k', facecolor='k') #轮子对象
        self.wheel_FR = patches.Polygon(self.wheel_shape, closed=True,lw = 1, edgecolor='k', facecolor='k')
        self.wheel_BL = patches.Polygon(self.wheel_shape, closed=True,lw = 1, edgecolor='k', facecolor='k')
        self.wheel_BR = patches.Polygon(self.wheel_shape, closed=True,lw = 1, edgecolor='k', facecolor='k')

        self.x = []  #轨迹点
        self.y = []
        self.trajectory, = self.ax.plot([], [], 'r-')#轨迹线对象，要加逗号

    def update_graph(self,frame): #图形更新

        body_points = rotate_translate(self.body_points, self.yaw, self.pos)  #旋转平移
        self.body.set_xy(body_points)   #设定图形坐标
       
        wheel_points_FL = rotate_translate(self.wheel_shape, self.angle_wheel, [-self.Width/2, self.Len])  #旋转平移至相对车身对应位置      
        wheel_points_FL = rotate_translate(wheel_points_FL, self.yaw, self.pos)   #旋转平移至车身对应位置

        wheel_points_FR = rotate_translate(self.wheel_shape, self.angle_wheel, [self.Width/2, self.Len])
        wheel_points_FR = rotate_translate(wheel_points_FR, self.yaw, self.pos)

        wheel_points_BL = rotate_translate(self.wheel_shape, 0, [-self.Width/2, 0])
        wheel_points_BL = rotate_translate(wheel_points_BL, self.yaw, self.pos)

        wheel_points_BR = rotate_translate(self.wheel_shape, 0, [self.Width/2, 0])
        wheel_points_BR = rotate_translate(wheel_points_BR, self.yaw, self.pos)

        self.wheel_FL.set_xy(wheel_points_FL) #设定坐标
        self.wheel_FR.set_xy(wheel_points_FR)
        self.wheel_BL.set_xy(wheel_points_BL)
        self.wheel_BR.set_xy(wheel_points_BR)
        
        plt.draw()
        # 清空坐标轴
        ax.clear()
        
        # 添加图形到坐标轴
        
        self.ax.add_artist(self.body)
        self.ax.add_artist(self.wheel_FL)
        self.ax.add_artist(self.wheel_FR)
        self.ax.add_artist(self.wheel_BL)
        self.ax.add_artist(self.wheel_BR)
        self.ax.add_artist(self.trajectory)

        self.x.append(self.pos[0][0]) #添加轨迹点
        self.y.append(self.pos[0][1])
        self.trajectory.set_data(self.x,self.y)
        

    def update_state(self,step):  #更新状态数据
        self.yaw_w = self.vy*tan(self.angle_wheel)/self.Len #计算车身航向角速度

        self.yaw = self.yaw + self.yaw_w*step               #计算车身航向角
        v = rotate_translate((0,self.vy), self.yaw, [0,0])
        self.pos = self.pos +  np.array(v)*step             #计算车身位置
        
        # print("pos:",self.pos)
t1 = AkmanCar(ax,0)
t = 0.1
def updateall(frame):
    global t
    ax.clear()
    t = t+0.04
    t1.angle_wheel = 0.6*cos(t)
    t1.vy = 5
    t1.update_state(0.1)
    t1.update_graph(frame)

    # 设置坐标轴范围
    t1.ax.set_xlim(t1.pos[0][0] - 10, t1.pos[0][0] + 10)
    t1.ax.set_ylim(t1.pos[0][1] - 10, t1.pos[0][1] + 10)


def main():  
        
    # 创建动画
    animation = FuncAnimation(fig, updateall, frames=range(100), interval=1)
    # 显示动画
    plt.show()

if __name__ == '__main__':
    main()