胡萝卜路径追踪算法: 当针对关键任务启动无人系统时,需要遵循预定路径。这意味着无人系统需要遵循路径的算法来完成任务。该算法使用比例控制器形式的简单控制器来控制无人系统的移动。这篇文章基于python实现了该算法。可直接复制运行。 关于该算法的详细介绍,建议参考这篇论文: Path Planning of Unmanned System using Carrot-chasing Algorithm
from math import atan
import matplotlib.pyplot as plt
import numpy as np
Va = 25
phi = 0.9
Vx = Va * np.cos(phi)
Vy = Va * np.sin(phi)
Wa = (6, 65)
Wb = (12, 35)
Px = 10
Py = 32
plt.plot(Px, Py, 'pr')
plt.plot(Wa, Wb, '-d')
Ru = np.sqrt((Wb[0] - Py) ** 2 + (Wa[0] - Px) ** 2)
theta = abs(atan(float(Wb[1] - Wb[0]) / float(Wa[1] - Wa[0])))
thetaU = abs(atan((Py - Wb[0]) / (Px - Wa[0])))
beta = abs(theta - thetaU)
R = Ru * np.cos(beta)
e = Ru * np.sin(beta)
plt.plot((Wa[0], Px), (Wb[0], Py), '-g')
delta = 5
xt = Wa[0] + (R + delta) * np.cos(theta)
yt = Wb[0] + (R + delta) * np.sin(theta)
plt.plot(xt, yt, '^r')
K = 0.5
K2 = 35
while abs(e) > 0:
t = 0.05
phiD = abs(atan((yt - Py) / (xt - Px)))
u = K * (phiD - phi) * Va - K2 * e
if u > 1:
u = 1
phi = phiD
Vy = Va * np.sin(phi) + u * t
Vx = np.sqrt(Va * Va - Vy * Vy)
Px = Px + Vx * t
Py = Py + Vy * t
t = t + 0.1
plt.plot(Px, Py, 'pr ')
plt.pause(0.1)
Ru = np.sqrt((Wb[0] - Py) ** 2 + (Wa[0] - Px) ** 2)
thetaU = abs(atan((Py - Wb[0]) / (Px - Wa[0])))
beta = abs(theta - thetaU)
R = Ru * np.cos(beta)
e = Ru * np.sin(beta)
plt.plot((Wa[0], Px), (Wb[0], Py), '--g ')
xt = Wa[0] + (R + delta) * np.cos(theta)
yt = Wb[0] + (R + delta) * np.sin(theta)
plt.plot(xt, yt, '^b')
print xt, yt
plt.show()
|