无人驾驶-控制-Stanley
无人驾驶-控制-Stanley
一、模型介绍
1.1 简介
关于无人车追踪轨迹，目前的主流方法分为两类：基于几何追踪的方法和基于模型预测的方法，其中几何追踪方法主要包含纯跟踪和Stanley两种方法，纯跟踪方法已经广泛应用于移动机器人的路径跟踪中，网上也有很多详细的介绍，本文主要介绍斯坦福大学无人车使用的Stanley方法。
前面介绍的Pure Pursuit方法利用无人车的横向跟踪误差来设计控制器。本篇介绍的Stanely方法就结合横向跟踪误差ey与航向角偏差eθ来设计控制器。值得注意的是，Stanley计算横向跟踪误差与pure pursuit方法是有区别的，Pure pursuit是以后轴中心为基准点计算几何学公式，而Stanley是基于前轴中心为基准点计算几何学公式的。
二、 公式推导
2.1 车辆总的前轮期望转角
Stanley方法是一种基于横向跟踪误差（cross-track error：e为前轴中心到最近路径点(px ,py)的距离）的非线性反馈函数，并且能实现横向跟踪误差指数收敛于0。根据车辆位姿与给定路径的相对几何关系可以直观的获得控制车辆方向盘转角的控制变量，其中包含横向误差e和航向误差θe 。
前轮转角由横向和航向误差求取相加：δ(t)=δ_e(t)+δ_θe(t)
2.2 航向误差引起的前轮转角
不考虑横向误差，单考虑航向误差：
在不考虑横向跟踪误差e的情况下，前轮偏角和给定路径切线方向一致，如图所示。其中θe表示车辆航向与最近路径点切线方向之间的夹角，在没有任何横向误差的情况下，前轮方向与所在路径点的方向相同。不考虑横向误差，车辆航向角与目标点切线方向夹角θe，则只需要将车前轮偏转δ_θe，则能弥补航向误差。
航向误差引起的前轮转角与航向误差等式：δ_θe(t) = θe(t)
2.3 横向误差引起的前轮转角
不考虑航向误差，单考虑横向误差：
在不考虑航向跟踪误差θe的情况下，横向跟踪误差越大，前轮转向角越大，假设车辆预期轨迹在距离前轮d(t)处与给定路径上最近点切线相交，根据几何关系得出如下非线性比例函数：
其中d(t)与车速相关，最后用车速v(t)，增益参数k表示。随着横向误差的增加，arctan函数产生一个直接指向期望路径的前轮偏角，并且收敛受车速=v(t)限制。
2.4 “横向误差+航向误差”前轮转角之和
综合两方面控制因素，基本转向角控制量如下：
使用线性自行车运动模型，可以得到横向误差的变化率：
sin(δ_e(t))------>e’(t)------->e(t)
因此横向误差指数收敛于e(t)=0，参数k决定了收敛速度。对于任意横向误差，微分方程都单调的收敛到0。
三、代码分析
3.1 求解航向角
由于该部分代码与Pure_Persuit算法的代码有较大相似之处，因此这里只讲解核心部分calculateTwistCommand()函数及其不同之处。
lad = 0.0 #求解航向角
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算法基本相同，大致分为三个小部分，因此只简述其中每一小部分的作用。
- 第一部分的主要作用是找到距离当前位置最近的局部路径点的横、纵坐标。
- 第二部分的主要作用是获取该局部路径点对应的目标航向角。
- 第三部分的主要作用是获取小车当前的航向角。
3.2 求解当前点与目标点航向误差
#get 求解航向角度差
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.3 求解当前点和目标点横向误差
#get 求解当前点和目标点横向误差
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- 该部分的主要作用是计算得到小车前轮中心轴位置与目标路径点之间的横向误差。
3.4 非线性比例函数
#get 非线性比例函数
if vel < 0.001:
delta = 0
else:
delta = math.atan(k * error / vel)
print 'delta = ', delta- 该部分的主要作用是根据几何关系得出一个非线性比例函数，从而产生一个直接指向期望路径的前轮偏角delta，并且收敛受车速v(t)限制。
3.5 终点处判断处理
#get 求最终点和当前点距离
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- 该部分的主要作用是获取小车当前位置与目标位置之间的距离，若小车当前位置与终点位置(0,4)的距离小于0.1m，则小车到达终点，此时停止运动。