1. 算法简介

Stanley方法是一种基于横向跟踪误差（cross-track error：$e$为前轴中心到最近路径点($p_x$​,$p_y$​)的距离）的非线性反馈函数，并且能实现横向跟踪误差指数收敛于0。根据车辆位姿与给定路径的相对几何关系可以直观的获得控制车辆方向盘转角的控制变量，其中包含横向误差e和航向误差${\theta }_e$​。
$$\delta (t)={\delta }_e(t)+{\delta }_{{\theta }_e}(t)$$在不考虑横向跟踪误差$e$的情况下，前轮偏角和给定路径切线方向一致，如图所示。其中${\theta }_e$​表示车辆航向与最近路径点切线方向之间的夹角，在没有任何横向误差的情况下，前轮方向与所在路径点的方向相同：
$${\delta }_{{\theta }_e}(t)={\theta }_e(t)$$在不考虑航向跟踪误差${\theta }_e$​的情况下，横向跟踪误差越大，前轮转向角越大，假设车辆预期轨迹在距离前轮$d(t)$处与给定路径上最近点切线相交，根据几何关系得出如下非线性比例函数：
$${\delta }_e(t)=arctan\frac{e(t)}{d(t)}=arctan\frac{ke(t)}{v(t)}$$
其中$d(t)$与车速相关，最后用车速$v(t)$，增益参数k表示。随着横向误差的增加，arctan函数产生一个直接指向期望路径的前轮偏角，并且收敛受车速$v(t)$限制。
综合两方面控制因素，基本转向角控制率如下：
$$\delta (t)={\theta }_e(t)+arctan\frac{ke(t)}{v(t)}$$使用线性自行车运动模型，可以得到横向误差的变化率：
$$\dot{e}(t)=-v(t)sin{\delta }_e(t)$$其中$sin{\delta }_e(t)$根据几何关系可知：
$$sin{\delta }_e(t)=\frac{e(t)}{\sqrt{d(t{)}^2+(e(t){)}^2}}=\frac{ke(t)}{\sqrt{v(t{)}^2+(ke(t){)}^2}}$$
$$\dot{e}(t)=\frac{-v(t)ke(t)}{\sqrt{v(t{)}^2+(ke(t){)}^2}}=\frac{-ke(t)}{\sqrt{1+(\frac{ke(t)}{v(t)}{)}^2}}$$当横向跟踪误差$e(t)$很小时，$(\frac{ke(t)}{v(t)}{)}^2\to 0$:
$$\dot{e}(t)\approx -ke(t)$$通过积分上式:
$$e(t)=e(0)\times e^{-kt}$$因此横向误差指数收敛于e(t)=0，参数k决定了收敛速度。对于任意横向误差，微分方程都单调的收敛到0。

2. 源码解析

Note: 由于该部分代码与Pure_Persuit算法的代码有较大相似之处，因此这里只讲解核心部分calculateTwistCommand()函数及其不同之处。
(1) calculateTwistCommand()函数part.1

lad = 0.0 #look ahead distance accumulator
targetIndex = len(self.currentWaypoints.waypoints) - 1
for i in range(len(self.currentWaypoints.waypoints)):
    if((i+1) < len(self.currentWaypoints.waypoints)):
        this_x = self.currentWaypoints.waypoints[i].pose.pose.position.x
        this_y = self.currentWaypoints.waypoints[i].pose.pose.position.y
        next_x = self.currentWaypoints.waypoints[i+1].pose.pose.position.x
        next_y = self.currentWaypoints.waypoints[i+1].pose.pose.position.y
        lad = lad + math.hypot(next_x - this_x, next_y - this_y)
        if(lad > HORIZON):
            targetIndex = i+1
            break

targetWaypoint = self.currentWaypoints.waypoints[targetIndex]

targetSpeed = self.currentWaypoints.waypoints[0].twist.twist.linear.x

targetX = targetWaypoint.pose.pose.position.x
targetY = targetWaypoint.pose.pose.position.y
currentX = self.currentPose.pose.position.x
currentY = self.currentPose.pose.position.y
#print ‘[‘,targetX, targetY, ‘]’
#print targetWaypoint.pose.pose.orientation
print self.currentVelocity

#get waypoint yaw angle
waypoint_quanternion = (targetWaypoint.pose.pose.orientation.x, targetWaypoint.pose.pose.orientation.y, targetWaypoint.pose.pose.orientation.z, targetWaypoint.pose.pose.orientation.w)
waypoint_euler = tf.transformations.euler_from_quaternion(waypoint_quanternion)
TargetYaw = waypoint_euler[2]
print ‘TargetYaw = ‘, TargetYaw

#get vehicle yaw angle
quanternion = (self.currentPose.pose.orientation.x, self.currentPose.pose.orientation.y, self.currentPose.pose.orientation.z, self.currentPose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quanternion)
CurrentYaw = euler[2]
print ‘CurrentYaw = ‘, CurrentYaw

说明：

• 这部分与Pure_Persuit算法基本相同，大致分为三个小部分，因此只简述其中每一小部分的作用。
• 第一部分的主要作用是找到距离当前位置最近的局部路径点的横、纵坐标。
• 第二部分的主要作用是获取该局部路径点对应的目标航向角。
• 第三部分的主要作用是获取小车当前的航向角。

(2) calculateTwistCommand()函数part.2

#get the difference between yaw angle of vehicle and tangent line of waypoint
if TargetYaw >= 0:
    alpha = TargetYaw - CurrentYaw
    flag_alpha = 1
else:
    if TargetYaw _ CurrentYaw < 0:
        if (TargetYaw < 0) and (CurrentYaw > 0):
            alpha = (math.pi - CurrentYaw) + (math.pi + TargetYaw)
            flag_alpha = 2
        else:
            alpha = -((math.pi + CurrentYaw) + (math.pi - TargetYaw))
            flag_alpha = 3
    else:
        alpha = TargetYaw - CurrentYaw
        flag_alpha = 4

print ‘flag_alpha = ‘, flag_alpha
print ‘(‘, currentX, currentY, ‘)’
print ‘(‘, targetX, targetY, ‘)’
print ‘alpha = ‘, alpha
print ‘x = ‘, targetX - currentX
print ‘y = ‘, targetY - current

说明：

• 该部分的主要作用是计算得到目标航向角与当前实际航向角之间的航向误差alpha。

(3) calculateTwistCommand()函数part.3

#get the error between target position and current position
if alpha >= 0:
    error = abs( math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2)) )
else:
    error = -abs( math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2)) )
print ‘error = ‘, error

说明：

• 该部分的主要作用是计算得到小车前轮中心轴位置与目标路径点之间的横向误差error。

(4) calculateTwistCommand()函数part.4

#get the velocity of vehicle
vel = math.sqrt(math.pow(self.currentVelocity.twist.linear.x, 2) + math.pow(self.currentVelocity.twist.linear.y, 2))
print ‘vel = ‘, vel

说明：

• 该部分的主要作用是获取小车在当前航向角方向上的速度值。

(5) calculateTwistCommand()函数part.5

#get the nonlinear proportional function from geometry of Vehicular mobility model
if vel < 0.001:
    delta = 0
else:
    delta = math.atan(k _ error / vel)
print ‘delta = ‘, delta

说明：

• 该部分的主要作用是根据几何关系得出一个非线性比例函数，从而产生一个直接指向期望路径的前轮偏角delta，并且收敛受车速v(t)限制。

(6) calculateTwistCommand()函数part.6

#get the distance between final position and current position
l = math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2))
l2 = math.sqrt(math.pow(currentX - final_targetX, 2) + math.pow(currentY - final_targetY, 2))

if l > 0.001:
    if l2 > 0.1:
        theta = alpha + delta
        print ‘theta = ‘, theta
        #self.ackermann_steering_control(targetSpeed, theta)
        self.ackermann_steering_control(15, -theta)

        #get twist command
        left_steer = Float64()
        right_steer = Float64()
        left_vel = Float64()
        right_vel = Float64()
        left_steer.data = left_angle
        right_steer.data = right_angle
        left_vel.data = left_speed
        right_vel.data = right_speed
        flag = 1
    else:
        left_steer = Float64()
        right_steer = Float64()
        left_vel = Float64()
        right_vel = Float64()
        left_steer.data = 0
        right_steer.data = 0
        left_vel.data = 0
        right_vel.data = 0
        flag = 2
else:
    left_steer = Float64()
    right_steer = Float64()
    left_vel = Float64()
    right_vel = Float64()
    left_steer.data = 0
    right_steer.data = 0
    left_vel.data = 0
    right_vel.data = 0
    flag = 3

print ‘flag = ‘, flag

return left_vel, right_vel, left_steer, right_steer

• 45

说明：

• 该部分的主要作用是获取小车当前位置与目标位置之间的距离，若小车当前位置与终点位置(0,4)的距离小于0.1m，则小车到达终点，此时停止运动。

3. 效果演示