【[第一次写博客]Uda课程中的P控制器实现说明】

---

前言

提示：这里可以添加本文要记录的大概内容：

面试官如果正在面我，请让我过吧。（手动狗头）

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、博客目的？

记录下自己学习的过程，跟大家一起交流。

二、Uda课程中PID控制算法的Python实现

1.引入库

后面有用到，再一起看一下。代码如下：

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

还有一条

# maximum recursion depth exceeded
import sys
sys.setrecursionlimit(3000)

这个咱也不懂啊，我把sys.setrecursionlimit,doc(我记得有个markdown段内插入代码的功能，忘了，后面再加吧。)搬过来了，能看懂一点点了。简言之，防止爆栈。

    setrecursionlimit(n)
    
    Set the maximum depth of the Python interpreter stack to n.  This
    limit prevents infinite recursion from causing an overflow of the C
    stack and crashing Python.  The highest possible limit is platform-
    dependent.

2.class和运行

这个还是稍微麻烦，我直接在注释中写好，然后把代码复制过来，怎么高效怎么来。：

class Robot(object):
    def __init__(self, length=20.0):
        """
        Creates robot and initializes location/orientation to 0, 0, 0.
        我来翻译一下，创建机器人并初始化位置和方向为0， 0， 0
        """
        self.x = 0.0
        self.y = 0.0
        self.orientation = 0.0 #todo 这个角度应该是车辆x轴和大地X轴的角度，这里存疑
        self.length = length
        self.steering_noise = 0.0
        self.distance_noise = 0.0
        self.steering_drift = 0.0

    def set(self, x, y, orientation):
        """
        Sets a robot coordinate.
        设置一个机器人的坐标。位置和方向。
        """
        self.x = x
        self.y = y
        self.orientation = orientation % (2.0 * np.pi) #这里是个取余的符号，我之前看成了除法，纳闷了大半天，果然不能自以为是

    def set_noise(self, steering_noise, distance_noise):
        """
        Sets the noise parameters.
        设置噪声参数，噪声无处不在啊。
        """
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        self.steering_noise = steering_noise
        self.distance_noise = distance_noise

    def set_steering_drift(self, drift):
        """
        Sets the systematical steering drift parameter
        设置系统的转向漂移参数。这里面可能考虑的是转向齿轮之前的框量，也应该包括初始时候的偏移。
        """
        self.steering_drift = drift

    def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0):
        """
        steering = front wheel steering angle, limited by max_steering_angle
        distance = total distance driven, most be non-negative
        转向是前轮转向角度，受限。不能超过物理极限。
        距离一般是非负的。
        """
        if steering > max_steering_angle:#转向角过大的处理
            steering = max_steering_angle
        if steering < -max_steering_angle:
            steering = -max_steering_angle
        if distance < 0.0:#形势距离为负的处理
            distance = 0.0

        # apply noise 噪声值   貌似高斯噪声是符合正太分布的。属于我的知识盲区，后面要补上。
        steering2 = random.gauss(steering, self.steering_noise)
        distance2 = random.gauss(distance, self.distance_noise)

        # apply steering drift 噪声值再加上漂移值
        steering2 += self.steering_drift

        # Execute motion 不知道用的啥模型，我去找老王推到运动学模型去了。先看下面的。
        turn = np.tan(steering2) * distance2 / self.length

        if abs(turn) < tolerance:
            # approximate by straight line motion
            self.x += distance2 * np.cos(self.orientation)
            self.y += distance2 * np.sin(self.orientation)
            self.orientation = (self.orientation + turn) % (2.0 * np.pi)
        else:
            # approximate bicycle model for motion
            radius = distance2 / turn
            cx = self.x - (np.sin(self.orientation) * radius)
            cy = self.y + (np.cos(self.orientation) * radius)
            self.orientation = (self.orientation + turn) % (2.0 * np.pi)
            self.x = cx + (np.sin(self.orientation) * radius)
            self.y = cy - (np.cos(self.orientation) * radius)

    def __repr__(self):
        return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)

############## ADD / MODIFY CODE BELOW ####################
# ------------------------------------------------------------------------
#
# run - does a single control run
robot = Robot() #实例化
robot.set(0, 1, 0) #对象设置初始值

def run(robot, tau, n=100, speed=1.0):
    x_trajectory = []
    y_trajectory = []
    # TODO: your codehere 下面是算法的实现
    for i in range(n):
        cte = robot.y
        steer = -tau*cte
        robot.move(steer,speed)
        x_trajectory.append(robot.x)
        y_trajectory.append(robot.y)

    return x_trajectory, y_trajectory
    
x_trajectory,y_trajectory = run(robot, 1) #第二个参数是P增益大小
n = len(x_trajectory)
print(x_trajectory)
print(y_trajectory)

#plt.plot(x_trajectory, y_trajectory, 'g', label='P controller')
#下面是出图，不知为啥没有显示出来
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8, 8))
ax1.plot(x_trajectory, y_trajectory, 'g', label='P controller')
ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference')

总结

亲爱的面试官，也许你发现我很菜，但谁还不是从新手慢慢熟起来的呢。我只想说，老司机，带带我。