📚 课程目录第1章 AGV运动学模型建立与仿真

1. AGV运动学模型建立与仿真

任务目标

建立差速驱动AGV的运动学模型，并在Python仿真环境中验证其圆弧与直线运动轨迹。通过给定左右轮速度，计算AGV位姿（x, y, θ）随时间的变化，并与理论轨迹对比。

环境依赖

仅需Python 3.8+，使用标准库math和matplotlib（用于可视化验证）。无需ROS或其他第三方库。

运动学模型

差速AGV模型基于以下假设：两驱动轮间距为L，轮半径为r，左右轮角速度分别为ω_L、ω_R。AGV线速度v与角速度ω为：

v = r·(ω_R + ω_L) / 2
ω = r·(ω_R − ω_L) / L

位姿更新（离散时间步长Δt）：

x' = x + v·cos(θ)·Δt
y' = y + v·sin(θ)·Δt
θ' = θ + ω·Δt

关键完整代码

import math
import matplotlib.pyplot as plt

class DifferentialAGV:
    """差速AGV运动学模型"""
    def __init__(self, wheel_radius=0.1, wheel_base=0.5, dt=0.05):
        self.r = wheel_radius          # 轮半径 (m)
        self.L = wheel_base            # 轮距 (m)
        self.dt = dt                   # 仿真步长 (s)
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0               # 航向角 (rad)

    def reset(self, x=0.0, y=0.0, theta=0.0):
        self.x, self.y, self.theta = x, y, theta

    def step(self, omega_L, omega_R):
        """输入左右轮角速度(rad/s)，更新位姿"""
        v = self.r * (omega_R + omega_L) / 2.0
        w = self.r * (omega_R - omega_L) / self.L
        self.x += v * math.cos(self.theta) * self.dt
        self.y += v * math.sin(self.theta) * self.dt
        self.theta += w * self.dt
        # 归一化到 [-pi, pi]
        self.theta = math.atan2(math.sin(self.theta), math.cos(self.theta))
        return self.x, self.y, self.theta

def simulate(agv, omega_L, omega_R, steps):
    """执行仿真，返回轨迹点列表"""
    traj = [(agv.x, agv.y)]
    for _ in range(steps):
        agv.step(omega_L, omega_R)
        traj.append((agv.x, agv.y))
    return traj

# ========== 仿真验证 ==========
if __name__ == "__main__":
    agv = DifferentialAGV(wheel_radius=0.1, wheel_base=0.5, dt=0.05)

    # 场景1：直线运动（左右轮同速）
    agv.reset()
    traj_line = simulate(agv, omega_L=10.0, omega_R=10.0, steps=200)
    # 理论终点：v = 0.1*10 = 1 m/s, t = 200*0.05 = 10s, 距离 = 10m
    print(f"直线终点: ({traj_line[-1][0]:.3f}, {traj_line[-1][1]:.3f})")
    print(f"理论终点: (10.000, 0.000)")

    # 场景2：圆弧运动（左右轮差速）
    agv.reset()
    traj_arc = simulate(agv, omega_L=8.0, omega_R=12.0, steps=300)
    # 理论：v = 0.1*10 = 1 m/s, w = 0.1*(4)/0.5 = 0.8 rad/s
    # 转弯半径 R = v/w = 1.25 m，圆心在左侧
    print(f"圆弧终点: ({traj_arc[-1][0]:.3f}, {traj_arc[-1][1]:.3f}, θ={agv.theta:.3f})")
    print(f"理论终点: (x=1.25*sin(4.0), y=1.25*(1-cos(4.0)), θ=4.000)")
    print(f"理论值:   ({1.25*math.sin(4.0):.3f}, {1.25*(1-math.cos(4.0)):.3f}, 4.000)")

    # 绘制轨迹
    plt.figure(figsize=(8, 4))
    plt.subplot(1, 2, 1)
    xs = [p[0] for p in traj_line]
    ys = [p[1] for p in traj_line]
    plt.plot(xs, ys, 'b-', label='仿真轨迹')
    plt.plot([0, 10], [0, 0], 'r--', label='理论直线')
    plt.axis('equal'); plt.grid(True); plt.legend(); plt.title('直线运动')

    plt.subplot(1, 2, 2)
    xs = [p[0] for p in traj_arc]
    ys = [p[1] for p in traj_arc]
    plt.plot(xs, ys, 'b-', label='仿真轨迹')
    # 理论圆弧
    theta_vals = [i*0.05*0.8 for i in range(301)]
    tx = [1.25*math.sin(t) for t in theta_vals]
    ty = [1.25*(1-math.cos(t)) for t in theta_vals]
    plt.plot(tx, ty, 'r--', label='理论圆弧')
    plt.axis('equal'); plt.grid(True); plt.legend(); plt.title('圆弧运动')
    plt.tight_layout()
    plt.show()

运行与验证

将代码保存为agv_kinematics.py，在终端运行：python agv_kinematics.py
程序输出两个场景的终点坐标，并弹出轨迹对比图。
通过条件：

直线场景：仿真终点x坐标与理论值10.000的误差小于0.01m（数值积分误差）；
圆弧场景：仿真终点与理论值误差小于0.05m，航向角误差小于0.01rad；
图中蓝色仿真轨迹与红色理论轨迹基本重合。

常见问题

轨迹发散：检查dt是否过大（建议≤0.05s），或左右轮速度差是否导致转弯半径过小。
航向角跳变：代码已做归一化处理，若自行修改需注意atan2的使用。
扩展阿克曼模型：将状态方程替换为v = r·ω_engine，ω = v·tan(δ)/L（δ为前轮转角），其余仿真框架不变。