import math
import numpy as np
import math
import matplotlib.pyplot as plt
class KinematicModel_3:
"""假设控制量为转向角delta_f和加速度a
"""
def __init__(self, x, y, psi, v, L, dt):
self.x = x
self.y = y
self.psi = psi
self.v = v
self.L = L
# 实现是离散的模型
self.dt = dt
def update_state(self, a, delta_f):
self.x = self.x+self.v*math.cos(self.psi)*self.dt
self.y = self.y+self.v*math.sin(self.psi)*self.dt
self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt
self.v = self.v+a*self.dt
def get_state(self):
return self.x, self.y, self.psi, self.v
k=0.2 # 增益系数
dt=0.1 # 时间间隔,单位:s
L=2 # 车辆轴距,单位:m
v = 2 # 初始速度
x_0=0 # 初始x
y_0=-3 #初始y
psi_0=0 # 初始航向角
def cal_target_index(robot_state, refer_path):
&#
按轨迹运行(stanley)
猜你喜欢
转载自blog.csdn.net/weixin_72050316/article/details/132153232
今日推荐
周排行