PID控制机器人侧向跟随人类

憨批CSDN编辑器，我编辑了半天一个ctrl+z想撤回一步，直接把我整篇文章撤回了

那就不写了，直接上代码（骂骂咧咧）

代码注释很详细了，希望n天后我回来还能看得懂

import numpy as np
import matplotlib.pyplot as plt
import math

class PID_posi_2:
    """位置式实现2"""
    def __init__(self, k=[1., 0., 0.], target=1.0, upper=1.0, lower=-1.0):
        self.kp, self.ki, self.kd = k
        # 初始化误差
        self.e = 0
        self.pre_e = 0 
        self.sum_e = 0 
        # 初始化目标值
        self.target = target
        # 初始化输出范围
        self.upper_bound = upper
        self.lower_bound = lower

    def cal_output(self, state): # 计算输出值
        self.e = state - self.target # 计算当前状态与目标状态的误差
        # 计算PID输出值
        u = self.e * self.kp + self.sum_e * self.ki + (self.e - self.pre_e) * self.kd
        # 判断输出角度输出值是否超出范围，如果超出范围则强制纠正
        if u < self.lower_bound:
            u = self.lower_bound
        elif u > self.upper_bound:
            u = self.upper_bound
        # 更新误差
        self.pre_e = self.e
        self.sum_e += self.e
        return u # 返回PID计算值

class KinematicModel_3:
    """定义一个三自由度的车辆运动模型假设控制量为转向角delta_f和速度v"""
    def __init__(self, x, y, psi, v, dt):
        self.x = x
        self.y = y
        self.psi = psi #航向角Ψ
        self.v = v
        # 实现是离散的模型
        self.dt = dt

    def update_state(self, v, delta_psi):
        """更新车辆状态"""
        self.psi = self.psi + delta_psi * self.dt
        self.v = v
        self.x = self.x+self.v*math.cos(self.psi)*self.dt
        self.y = self.y+self.v*math.sin(self.psi)*self.dt

def generate_human_trajectory(n_steps):
    # 定义每一步的转角和步长
    n_steps = n_steps - 1 #这样输入30就只生成30维的数据，而不是0-30共31维度
    angles = np.zeros(n_steps)
    distances = np.zeros(n_steps)

    # 生成每一步的转角和步长
    for i in range(n_steps):
        # 随机生成转角，范围在 [-pi/6, pi/6] 之间
        angle = (np.random.rand() - 0.5) * np.pi / 3
        # 如果两个相邻时刻的轨迹连线夹角超过 10 度，则重新生成转角
        while i > 0 and abs(np.degrees(np.arctan2(np.sin(angle - angles[i-1]), np.cos(angle - angles[i-1])))) > 10:
            angle = (np.random.rand() - 0.5) * np.pi / 3
        angles[i] = angle
        # 随机生成采样步长，范围在 [0.15, 0.2] 米之间，代表每1s采样得到的人类位置，不是每步的距离
        distance = 0.15 + np.random.rand() * 0.05
        distances[i] = distance

    # 根据转角和步长计算行走轨迹
    x = np.zeros(n_steps+1)
    y = np.zeros(n_steps+1)
    for i in range(1, n_steps+1):
        # 计算当前步的坐标
        x[i] = np.round(x[i-1] + distances[i-1] * np.cos(angles[i-1]),2)
        y[i] = np.round(y[i-1] + distances[i-1] * np.sin(angles[i-1]),2)

    return x, y # 返回每一时刻的轨迹点坐标

# 初始化角度PID控制器，输出角度范围[-pi*2.8, pi*2.8]，因为机器人每1s最大转2.8π
PID_angle = PID_posi_2(k=[1, 0.01, 0.01], target=0, upper=np.pi*2.8, lower=-np.pi*2.8)
# 初始化速度PID控制器，机器人最大速度为0.2m/s
PID_v = PID_posi_2(k=[0.3, 0, 0.01], target=0, upper=0.2, lower=0)

def main():
    # 假设初始状态为x=-1,y=0,偏航角=0rad，速度为0m/s，采样时间间隔为1秒
    Robot = KinematicModel_3(0, 0.5, 0, 0, 1)

    n_step = 100 #人类轨迹步数
    x_human, y_human = generate_human_trajectory(n_step) #生成人类随机轨迹
    human_trajectory_point = np.column_stack((x_human, y_human))
    d = 0.5# 设置安全距离d

    Robot_real_state = np.zeros((n_step,2)) #机器人实际轨迹坐标点
    robot_tracking_point = np.zeros((n_step,2))  #存储机器人待追踪点
    
    Robot_real_state[0:1] = [Robot.x, Robot.y] #记录机器人的初始化真实状态点
    robot_tracking_point[0:1] = [Robot.x, Robot.y] #添加第一个时刻的追踪点，就是机器人自身状态点

    for i in range(n_step-1):

        # 利用人类轨迹点(x,y)生成机器人待追踪轨迹点(x_tar,y_tar)
        if i > 0 :
            x_tar=np.round(x_human[i]+d*((y_human[i-1]-y_human[i])/np.sqrt((y_human[i-1]-y_human[i])**2+(x_human[i]-x_human[i-1])**2)),2)
            y_tar=np.round(y_human[i]+d*((x_human[i]-x_human[i-1])/np.sqrt((y_human[i-1]-y_human[i])**2+(x_human[i]-x_human[i-1])**2)),2)

            robot_tracking_point[i,:] = [x_tar, y_tar]
            # alpha是机器人与待追踪点的夹角
            alpha = math.atan2(robot_tracking_point[i][1]-Robot_real_state[i-1][1], robot_tracking_point[i][0]-Robot_real_state[i-1][0])
            l_d = np.linalg.norm(robot_tracking_point[i]-Robot_real_state[i-1]) # 前视距离：机器人中心离追踪目标点的距离
            theta_e = alpha-Robot.psi #psi是机器人自身航向角

            delta_psi = PID_angle.cal_output(theta_e) #用机器人与目标偏角计算航向角
            v = PID_v.cal_output(l_d)# 用前视距离计算速度
            
            Robot.update_state(v, delta_psi)
            Robot_real_state[i,:] = [Robot.x, Robot.y]

        plt.cla()
        plt.xlim([0, 20])  # 设置x轴范围
        plt.ylim([-5, 5])   # 设置y轴范围
        plt.plot(human_trajectory_point[0:i,0], human_trajectory_point[0:i,1], 'go-', markersize=2, label='Human')
        plt.plot(human_trajectory_point[i-1,0], human_trajectory_point[i-1,1], 'go', markersize=5)
        # plt.plot(robot_tracking_point[0:i,0], robot_tracking_point[0:i,1], 'ro', markersize=1) #绘制机器人待追踪路线点
        plt.plot(Robot_real_state[0:i,0], Robot_real_state[0:i,1], 'bo-', markersize=2, label='Robot')
        plt.plot(Robot_real_state[i-1,0], Robot_real_state[i-1,1], 'bo', markersize=5)
        plt.legend() 
        plt.grid(True) #显示网格线
        plt.pause(0.1)

    plt.show()

if __name__=='__main__':
    main()