В этой статье обсуждается реализация ПИД-регулятора на C ++ как часть программы наноразмеров Udacity.
Вступление
ПИД означает пропорционально-интегрально-производная. Эти три компонента объединены таким образом, что создается управляющий сигнал. Прежде чем приступить к реализации, давайте обсудим компоненты ПИД-регулятора. Я собираюсь обсудить, как использовать PID для углов поворота.
Ошибка кросс-трека
Поперечная ошибка - это расстояние автомобиля от траектории. Теоретически он лучше всего подходит для управления автомобилем, управляя рулем пропорционально погрешности поперечного сечения (CTE).
P компонент
Он устанавливает угол поворота пропорционально CTE с пропорциональным коэффициентом tau.
-tau * cteДругими словами, P, или пропорциональная, составляющая оказывала наиболее заметное влияние на поведение автомобиля. Это заставляет автомобиль поворачивать пропорционально (и противоположно) расстоянию от центра полосы движения (CTE) - если автомобиль далеко вправо, он резко поворачивает влево, если немного влево, он немного вправо. .
Компонент D
Это дифференциальный компонент контроллера, который помогает получить временную производную ошибки. Это означает, что когда автомобиль повернет достаточно, чтобы уменьшить ошибку, это поможет не пролететь мимо оси x.
Другими словами, компонент D, или дифференциал, противодействует тенденции компонента P звенеть и выходить за пределы центральной линии. Правильно настроенный параметр D заставит автомобиль плавно приближаться к центральной линии без звона.
diff_cte = cte - prev_cte
prev_cte = cte
- tau_d * diff_cteI компонент
Это интеграл или сумма ошибок для устранения систематических ошибок.
Другими словами, I, или интегральный, компонент противодействует смещению CTE, которое не позволяет контроллеру P-D достичь центральной линии. Это смещение может принимать несколько форм, например, дрейф рулевого управления, но я считаю, что в этой конкретной реализации компонент I, в частности, служит для уменьшения CTE вокруг кривых.
int_cte += cte
tau_i * int_cteВ сочетании с этим мы можем заставить ПИД-регулятор управлять значением рулевого управления.
cte = robot.y
diff_cte = cte - prev_cte
prev_cte = cte
int_cte += cte
steer = -tau_p * cte - tau_d * diff_cte - tau_i * int_cteОптимизацию параметров можно выполнить вручную или с помощью алгоритма Twiddle.
Псевдокод для реализации алгоритма Twiddle выглядит следующим образом:
function(tol=0.2) {
p = [0, 0, 0]
dp = [1, 1, 1]
best_error = move_robot()
loop untill sum(dp) > tol
loop until the length of p using i
p[i] += dp[i]
error = move_robot()
if err < best_err
best_err = err
dp[i] *= 1.1
else
p[i] -= 2 * dp[i]
error = move_robot()
if err < best_err
best_err = err
dp[i] *= 1.1
else
p[i] += dp[i]
dp[i] *= 0.9
return p
}Реализация ПИД-регулятора
Udacity предоставляет симулятор для выполнения проекта, и цель состоит в том, чтобы написать ПИД-регулятор для расчета значения рулевого управления для управления автомобилем с использованием Cross Track Error (CTE), предоставляемого симулятором.
Это очень простой контроллер в теории управления, использующий пропорциональные, интегральные и производные компоненты, использующий CTE для определения управляющего сигнала.
Псевдокод для определения алгоритма ПИД-регулятора на основе значения рулевого управления следующим образом:
cte = robot.y
diff_cte = cte - prev_cte
prev_cte = cte
int_cte += cte
steer = -tau_p * cte - tau_d * diff_cte - tau_i * int_cteКод проекта PID для определения значения рулевого управления на основе CTE:
void PID::Init(double Kp, double Ki, double Kd) { this->Kp = Kp; this->Ki = Ki; this->Kd = Kd; this->p_error = 0.0; this->i_error = 0.0; this->d_error = 0.0; }void PID::UpdateError(double cte) { d_error = cte - p_error; p_error = cte; i_error += cte; }double PID::TotalError() { return (-Kp * p_error) - (Ki * i_error) - (Kd * d_error); }void PID::Twiddle() {}
cte - это ошибка поперечного сечения, поступившая в контроллер от датчика.
p_error, i_error и d_error являются пропорциональными, интегральными и производные компоненты соответственно.
Kp, Ki, и Kd - это те параметры P, I и D, которые необходимо оптимизировать.
Нахождение начального значения Kp, Ki, Kd
Начальное значение для Kp, Ki, Kd выбирается с использованием метода отслеживания и ошибок. Это простой метод настройки ПИД-регулятора. В этом методе сначала мы должны установить значения Ki и Kd равными нулю и увеличивать пропорциональный член (Kp), пока система не достигнет колебательного поведения. Затем Kd был настроен на уменьшение колебаний, а затем Ki для уменьшения стационарной ошибки.
Автомобиль начал колебаться, когда значение KP было установлено на 0,05, тогда как Ki и Kd были установлены на ноль.
Вот видео с колеблющимся поведением:
Затем было найдено значение Kd, которое останавливает колебательное поведение, которое установлено на 1,5 вместе с 0,05 для Kp и нулем для Ki.
Вот видео после добавления значений Kp и Kd:
Наконец, значение Ki устанавливается равным 0,0001, чтобы уменьшить установившуюся ошибку.
Начальные значения для Kp, Ki и Kd устанавливаются следующим образом:
{0.05, 0.0001, 1.5}Twiddle
После установки начального значения алгоритм Twiddle используется для оптимизации параметров.
Я модифицировал main.cpp для реализации алгоритма Twiddle. Когда переменная twiddle установлена в значение true, симулятор запускает машину с конфиденциальностью до максимальных шагов, установленных изначально, и проходит алгоритм twiddle. После соревнования каждой итерации симулятор возвращается к начальному этапу, и машина запускается от начала до максимальных шагов. Этот процесс продолжается до тех пор, пока значение tol не станет ниже допустимого.
Оптимизированное значение мы получили, как показано ниже:
0.06, 0.00031, 1.29Как только я нашел оптимизированное значение, установите для переменной twiddle значение false, чтобы запустить машину через симулятор.
bool twiddle = false;Фактический алгоритм twiddle в Python выглядит следующим образом:
def twiddle(tol=0.2): p = [0, 0, 0] dp = [1, 1, 1] robot = make_robot() x_trajectory, y_trajectory, best_err = run(robot, p)it = 0 while sum(dp) > tol: print("Iteration {}, best error = {}".format(it, best_err)) for i in range(len(p)): p[i] += dp[i] robot = make_robot() x_trajectory, y_trajectory, err = run(robot, p)if err < best_err: best_err = err dp[i] *= 1.1 else: p[i] -= 2 * dp[i] robot = make_robot() x_trajectory, y_trajectory, err = run(robot, p)if err < best_err: best_err = err dp[i] *= 1.1 else: p[i] += dp[i] dp[i] *= 0.9 it += 1 return p
И мне пришлось изменить его для этого проекта и код следующим образом:
if (twiddle == true){ total_cte = total_cte + pow(cte,2); if(n==0){pid.Init(p[0],p[1],p[2]); } //Steering value pid.UpdateError(cte); steer_value = pid.TotalError();// DEBUG //std::cout << "CTE: " << cte << " Steering Value: " << steer_value << " Throttle Value: " << throttle_value << " Count: " << n << std::endl; n = n+1; if (n > max_n){//double sump = p[0]+p[1]+p[2]; //std::cout << "sump: " << sump << " "; if(first == true) { std::cout << "Intermediate p[0] p[1] p[2]: " << p[0] << " " << p[1] << " " << p[2] << " "; p[p_iterator] += dp[p_iterator]; //pid.Init(p[0], p[1], p[2]); first = false; }else{ error = total_cte/max_n;if(error < best_error && second == true) { best_error = error; best_p[0] = p[0]; best_p[1] = p[1]; best_p[2] = p[2]; dp[p_iterator] *= 1.1; sub_move += 1; std::cout << "iteration: " << total_iterator << " "; std::cout << "p_iterator: " << p_iterator << " "; std::cout << "p[0] p[1] p[2]: " << p[0] << " " << p[1] << " " << p[2] << " "; std::cout << "error: " << error << " "; std::cout << "best_error: " << best_error << " "; std::cout << "Best p[0] p[1] p[2]: " << best_p[0] << " " << best_p[1] << " " << best_p[2] << " "; }else{ //std::cout << "else: "; if(second == true) { std::cout << "Intermediate p[0] p[1] p[2]: " << p[0] << " " << p[1] << " " << p[2] << " "; p[p_iterator] -= 2 * dp[p_iterator]; //pid.Init(p[0], p[1], p[2]); second = false; }else { std::cout << "iteration: " << total_iterator << " "; std::cout << "p_iterator: " << p_iterator << " "; std::cout << "p[0] p[1] p[2]: " << p[0] << " " << p[1] << " " << p[2] << " "; if(error < best_error) { best_error = error; best_p[0] = p[0]; best_p[1] = p[1]; best_p[2] = p[2]; dp[p_iterator] *= 1.1; sub_move += 1; }else { p[p_iterator] += dp[p_iterator]; dp[p_iterator] *= 0.9; sub_move += 1; } std::cout << "error: " << error << " "; std::cout << "best_error: " << best_error << " "; std::cout << "Best p[0] p[1] p[2]: " << best_p[0] << " " << best_p[1] << " " << best_p[2] << " "; } }}if(sub_move > 0) { p_iterator = p_iterator+1; first = true; second = true; sub_move = 0; } if(p_iterator == 3) { p_iterator = 0; } total_cte = 0.0; n = 0; total_iterator = total_iterator+1;double sumdp = dp[0]+dp[1]+dp[2]; if(sumdp < tol) { //pid.Init(p[0], p[1], p[2]); std::cout << "Best p[0] p[1] p[2]: " << best_p[0] << best_p[1] << best_p[2] << " "; //ws.close(); //std::cout << "Disconnected" << std::endl; } else { std::string reset_msg = "42[\"reset\",{}]"; ws.send(reset_msg.data(), reset_msg.length(), uWS::OpCode::TEXT); }} else { msgJson["steering_angle"] = steer_value; msgJson["throttle"] = throttle_value; auto msg = "42[\"steer\"," + msgJson.dump() + "]"; ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT); }}
Если вы хотите увидеть полный код в действии, посетите мой репозиторий на github.
