Webots舵轮使用纯追踪算法

---

前言

对于AGV小车，利用路径规划算法在规划好路径以后，全局路径由一系列路径点构成，这些路径点只要包含空间位置信息即可，也可以包含姿态信息。在上一篇文章中，我们在webots中创建了舵轮小车，在本文中，我们将利用纯追踪算法(Pure Pursuit)来对舵轮小车进行路径点追踪。

---

一、自行车模型

上图为几何学自行车模型，假设我们的自行车模型仅在平面上行驶。我们将四轮模型简化为两轮模型，我们可以计算出前轮转向角度δ与轴距L之间的关系：

R为在给定的转向角δ下后轴遵循着的圆的半径。

二、纯追踪算法

纯跟踪算法以车后轴为切点，车辆纵向车身为切线，通过控制前轮转角，使车辆可以沿着一条经过目标路点的圆弧行驶，如上图所示。上图中(g_x,g_y )是我们下一个要追踪的路点，它位于我们已经规划好的全局路径上，现在需要控制车辆的后轴经过该路点，表示车辆当前位置（即后轴位置）到目标路点的距离，表示目前车身姿态和目标路点的夹角，那么更具正弦定理我们可以推导出如下转换式：

将上式变形可得：

K为圆弧的曲率，前轮的转向角δ：

将以上两个式子结合，并且把时间考虑进来可得：

t时刻车身和目标路点的夹角 α(t)和距离目标路点的前视距离ld的情况下，由于车辆轴距L固定，我们可以利用上式估计出应该作出的前轮转角δ，为了更好的理解纯追踪控制器的原理，我们定义el 为车辆当前姿态和目标路点在横向上的误差，由此可得夹角正弦：

此时，圆弧弧度为：

上式可知纯追踪控制器其实是一个横向转角的P控制器，其P系数为l_d^2，这个P控制器受到参数 ld（即前视距离）的影响很大，如何调整前视距离变成纯追踪算法的关键，通常来说，ld 被认为是车速的函数，在不同的车速下需要选择不同的前视距离。

三、Webots中对舵轮使用纯追踪算法

在Webots中新建一个控制器，代码如下：
代码如下（示例）：

import time
import numpy as np
from controller import *
import math

robot = Robot()

timestep = int(robot.getBasicTimeStep())

lf_dir = robot.getMotor('lf_dir_motor')
lf_val = robot.getMotor('lf_run_motor')

lb_dir = robot.getMotor('lb_dir_motor')
lb_val = robot.getMotor('lb_run_motor')

rf_dir = robot.getMotor('rf_dir_motor')
rf_val = robot.getMotor('rf_run_motor')

rb_dir = robot.getMotor('rb_dir_motor')
rb_val = robot.getMotor('rb_run_motor')

gps = robot.getGPS("gps")
gps.enable(timestep)

pen = robot.getPen("pen")

k = 0.00001
Lfc = 0.1
Kp = 1.0
dt = 0.032
L = 0.5

# tr_ = 180/3.1415926

def rotation(angle):
    lf_dir.setPosition(angle)
    lb_dir.setPosition(angle)
    rf_dir.setPosition(angle)
    rb_dir.setPosition(angle)


def translation():
    lf_val.setPosition(float('inf'))
    lf_val.setVelocity(leftSpeed)

    lb_val.setPosition(float('inf'))
    lb_val.setVelocity(leftSpeed)

    rf_val.setPosition(float('inf'))
    rf_val.setVelocity(rightSpeed)

    rb_val.setPosition(float('inf'))
    rb_val.setVelocity(rightSpeed)


class VehicleState:

    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v


def update(state, a, delta):
    state.x = state.x + state.v * math.cos(state.yaw) * dt
    state.y = state.y + state.v * math.sin(state.yaw) * dt
    state.yaw = state.yaw + state.v / L * math.tan(delta) * dt   
    # 航向角=后轴瞬时速度*tan前轮转角/轴距
    state.v = state.v + a * dt

    return state

def PControl(target, current):
    a = Kp * (target - current)

    return a


def calc_target_index(state, cx, cy):
    dx = [state.x - icx for icx in cx]
    dy = [state.y - icy for icy in cy]
    d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
    ind = d.index(min(d))  
    L = 0.0
    Lf = k * state.v + Lfc  

    while Lf > L and (ind + 1) < len(cx):
        dx = cx[ind + 1] - cx[ind]
        dy = cx[ind + 1] - cx[ind]
        L += math.sqrt(dx ** 2 + dy ** 2)
        ind += 1

    return ind


def pure_pursuit_control(state, cx, cy, pind):
    ind = calc_target_index(state, cx, cy)

    if pind >= ind:
        ind = pind

    if ind < len(cx):
        tx = cx[ind]
        ty = cy[ind]
    else:  
        tx = cx[-1]
        ty = cy[-1]
        ind = len(cx) - 1

    alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw  # 计算当前点到目标点的方向角差

    if state.v < 0:  
        alpha = math.pi - alpha

    Lf = k * state.v + Lfc  
    delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)  # 计算转向角

    return delta, ind

cx = np.arange(0, 50, 1)
cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx]

target_speed = 2.0 / 3.6

T = 1000.0

state = VehicleState(x=-0.0, y=-0.0, yaw=0.0, v=2.0)
lastIndex = len(cx) - 1
time = 0.0 
# print(state.x)
# print("Hello World")

x1= [state.x]
y1 = [state.y]
yaw1 = [state.yaw]
v1 = [state.v]
t1 = [0.0]

target_ind = calc_target_index(state, cx, cy)
    
while robot.step(timestep) != -1:
        
    ai = PControl(target_speed, state.v)
    di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
    print("di:", di)
    state = update(state, ai, di)
    print("yaw:", state.yaw)
    time = time + dt

    x1.append(state.x)
    y1.append(state.y)
    yaw1.append(state.yaw)
    v1.append(state.v)
    t1.append(time)

    values = gps.getValues()
    e_node = [-100000000000000, 60000000000000]
    x = values[0]
    y = values[1]
    if x > e_node[0] and y < e_node[1]:
        leftSpeed = 20.0
        rightSpeed = 20.0
    else:
        leftSpeed = 0.0
        rightSpeed = 0.0
    
    rotation(di)
    translation()
    pen.write(True)
    print("MY_ROBOT is at position: %g %g %g" % (values[0], values[1], values[2]))

---

更多内容请关注微信公众号:深度学习与路径规划

参考文献：https://blog.csdn.net/AdamShan/article/details/80555174