网站推广怎么做比较好,网站维护基础知识,lnmp.org wordpress,专业网站的建设设行吗写在前面#xff1a; #x1f31f; 欢迎光临 清流君 的博客小天地#xff0c;这里是我分享技术与心得的温馨角落。#x1f4dd; 个人主页#xff1a;清流君_CSDN博客#xff0c;期待与您一同探索 移动机器人 领域的无限可能。 #x1f50d; 本文系 清流君 原创之作 欢迎光临 清流君 的博客小天地这里是我分享技术与心得的温馨角落。 个人主页清流君_CSDN博客期待与您一同探索 移动机器人 领域的无限可能。 本文系 清流君 原创之作荣幸在CSDN首发 若您觉得内容有价值还请评论告知一声以便更多人受益。 转载请注明出处尊重原创从我做起。 点赞、评论、收藏三连走一波让我们一起养成好习惯 在这里您将收获的不只是技术干货还有思维的火花 系列专栏【运动控制】系列带您深入浅出领略控制之美。 愿我的分享能为您带来启迪如有不足敬请指正让我们共同学习交流进步 人生如戏我们并非能选择舞台和剧本但我们可以选择如何演绎 感谢您的支持与关注让我们一起在知识的海洋中砥砺前行~~~ 文章目录 引言一、Stanley 算法原理1.1 算法设计理念1.2 算法要求 二、转向角计算2.1 转向使航向与所需航向一致2.2 转向以消除横向误差2.3 转向角范围限制 三、Stanley 算法与 Pure Pursuit 算法对比3.1 相似点3.2 不同点 四、误差对转向角的影响4.1 对于较大的航向误差4.2 对于较大的横向误差 五、Stanley 算法稳定性分析5.1 横向误差变化率公式5.2 相轨迹图 六、Stanley 算法优化6.1 低速情况下的噪声软化6.2 高速情况下的航向阻尼6.3 引入前馈项6.4 综合优化 七、Stanley 算法的控制直观性7.1 直接调整航向角误差和横向误差 八、Stanley 算法的调节陷阱8.1 比例系数增大会导致不稳定8.2 避免特定场景的过度调节8.3 与纯跟踪算法相比的转弯过冲现象 九、编程实现9.1 Python 实现9.2 Matlab 实现 十、总结参考资料 引言 基于几何模型的轨迹追踪控制器有两种算法Pure Pursuit 算法和Stanley 算法。 Pure Pursuit 算法是根据前视参考点来定义转向输入的。 本篇博客介绍第二种Stanley 算法。 斯坦福大学运用基于 Stanley 算法的控制器赢得了第二届 DARPA 挑战赛冠军。Stanley 算法是一种基于几何路径的轨迹追踪控制器原理非常简单但对移动机器人和车辆控制都很有用。 一、Stanley 算法原理
1.1 算法设计理念 Stanley 算法是一种基于几何路径的轨迹追踪控制器其设计理念是将参考点设置在前轮中心而非传统的重心、质心或后轮重心。这种设计方式使得该算法能够在路径追踪过程中实现全局收敛并且误差衰减速率不随车辆速度变化而改变。实验结果表明以前轮中心为参考点的控制器性能优于以重心或后轮重心为参考点的控制器。 在没有前视距离的情况下直接根据前轮中心参考点来考虑横向误差以及航向角误差。 定义在前轮寻找到距离前轮中心最近的路径点其切线方向与车辆横摆角方向间的差别为航向角误差 θ e \theta_e θe前轮中心与最近路径点两点连线距离为横向误差 e e e。
1.2 算法要求
Stanley 算法是非常直观的控制器有以下三个要求
矫正航向误差矫正位置误差将转向角限制在最大转向角范围之内 二、转向角计算
下面具体看一下怎样根据这三个要求计算转向角。
2.1 转向使航向与所需航向一致 首先为了消除相对路径的航向误差可直接将转向角设置为等于转向误差。 δ ( t ) θ e ( t ) \delta ( t ) \theta _ { e } ( t ) δ(t)θe(t) 假设车辆没有横向误差 即 e 0 e0 e0 时前轮只需转过 θ e \theta _ { e } θe 的角度就可以完全追踪上参考轨迹。
2.2 转向以消除横向误差 为了消除横向误差 e e e可以构建下图所示的三角形 假设前轮中心为 A A A取前轮中心最近的点为 C C C沿速度方向的交点得到 B B B。这样可以看出由横向误差所产生的 ∠ A B C δ e \angle ABC\delta_e ∠ABCδe定义 B C ‾ d ( t ) \overline{BC}d\left( t \right) BCd(t)可得 tan δ e e d \tan \delta _e\frac{e}{d} tanδede 其中 d d d 是与速度有关的量加入与速度相反的比例系数 k k k则 d v k d\frac{v}{k} dkv这样前轮转角误差就可写成 tan δ e k e v \tan \delta _e \frac { k e } { v } tanδevke所以 δ ( t ) δ e ( t ) tan − 1 ( k e ( t ) v ( t ) ) \delta ( t ) \delta _ { e } ( t ) \tan ^ { - 1 } ( \frac { k e ( t ) } { v ( t ) } ) δ(t)δe(t)tan−1(v(t)ke(t)) 由于 δ e \delta _ { e } δe 是反正切函数所以其缩放可以把它控制到 [ − π , π ] [-\pi,\pi] [−π,π] 角度范围内。 比例系数 k k k 可根据实验调整即根据不同场景选择不同的 k k k。
2.3 转向角范围限制 转向角始终保持在最小值和最大值之间的范围内 δ ( t ) ∈ [ δ min , δ max ] \delta ( t ) \in \left[ \delta _ { \min } , \delta _ { \max } \right] δ(t)∈[δmin,δmax] 通常 δ min \delta _ { \min } δmin 和 δ max \delta _ { \max } δmax 和 0 0 0 对称即下图所示的形式 这样就可根据这三个考虑的因素形成最终的控制律 Stanley 算法的最终表达形式其实就是简单的将这三个要素结合在一起。 δ ( t ) θ e ( t ) tan − 1 ( k e ( t ) v ( t ) ) \delta ( t ) \theta _ { e } ( t ) \tan ^ { - 1 } ( \frac { k e ( t ) } { v ( t ) } ) δ(t)θe(t)tan−1(v(t)ke(t))其中 δ ( t ) ∈ [ δ min , δ max ] \delta ( t ) \in \left[ \delta _ { \min } , \delta _ { \max } \right] δ(t)∈[δmin,δmax] 最终的前轮转角 δ ( t ) \delta ( t ) δ(t) 等于对导航角误差 θ e ( t ) \theta_e(t) θe(t) 的补偿对横向误差 e ( t ) e(t) e(t) 的补偿最后把前轮转角控制在最大最小的范围之内。 三、Stanley 算法与 Pure Pursuit 算法对比
3.1 相似点
对比之前学过的 Pure Pursuit 纯跟踪算法有很多相似点 基于几何模型 都是基于车辆几何模型执行车辆横向控制任务。 包含速度项 具有非常类似的通过前进速度调整增益的特性即包含速度项。 比例控制信号 具有非常相似的比例控制信号
3.2 不同点 Stanley 算法对航向误差和横向误差分别有独立的惩罚 Stanley 算法不需要寻找目标点不需要前视距离
这就是 Stanley 算法和 Pure Pursuit 纯跟踪算法最大的区别。 四、误差对转向角的影响 下面来看一下不同的误差信号要求的转向角有多少。
4.1 对于较大的航向误差 控制器调用最大转向命令直至车头朝向回到平衡范围内 首先来看这样的场景当导航角误差非常大且转向指令与导航角方向相反时 可以看到车头朝向和参考轨迹的角度差为 θ e \theta_e θe。这时前轮转角已经非常大要让车辆转角纠正与路径的错位就会有很大的航向误差。 当航向误差 ∣ θ e ∣ δ max | \theta_e | \delta _ { \max } ∣θe∣δmax 时控制器会一直调用最大转向命令直到车头朝向回到平衡范围内。
4.2 对于较大的横向误差 控制器使转向角保持在最大值附近直至横向误差减小。 第二种情况是对于很大的横向误差假设初始时航向误差为 0 0 0即 θ e ≈ 0 \theta_e \approx 0 θe≈0 此时 tan − 1 ( k e ( t ) v f ( t ) ) ≈ π 2 \tan ^ { - 1 } ( \frac { k e ( t ) } { v _ { f } ( t ) } ) \approx \frac { \pi } { 2 } tan−1(vf(t)ke(t))≈2π 假如车辆非常接近参考路径即 θ e \theta_e θe 比较小此时主要的目标就是让车够靠近参考轨迹这时 Stanley 算法会使转向角保持在相当大的最大值上 δ ( t ) ≈ θ e ( t ) π 2 \delta ( t ) \approx \theta _ { e } ( t ) \frac { \pi } { 2 } δ(t)≈θe(t)2π 使得车辆不断地向参考轨迹靠近即车辆会不断地转向参考路径而这样会增加相反方向的航向误差 θ e \theta_e θe。 当转向角开始以最大的方向往参考轨迹转时车就会变成下图这样航向误差在增加直至当航向误差超过 − π 2 -\frac{\pi}{2} −2π 时前轮转角会到达平衡状态即航向误差和横向误差相互抵消。 此时前轮转角 δ ( t ) ≈ 0 \delta(t)\approx 0 δ(t)≈0 保持直行到参考路径直到横向误差减小这时才开始再次矫正与路径对齐。 最终才能让车辆开始更紧密的追踪路径。 五、Stanley 算法稳定性分析 下面对 Stanley 算法的稳定性进行分析。
5.1 横向误差变化率公式 首先建立横向误差变化率 当车辆不在最大转角时误差率可以写成这种形式 e ˙ ( t ) − v f ( t ) sin ( δ e ) − v f ( t ) sin ( tan − 1 k e ( t ) v f ( t ) ) \dot { e } ( t ) - v _ { f } ( t ) \sin ( \delta _ { e } ) - v _ { f } ( t ) \sin ( \tan ^ { - 1 } \frac { k e ( t ) } { v _ { f } ( t ) } ) e˙(t)−vf(t)sin(δe)−vf(t)sin(tan−1vf(t)ke(t)) 假设 δ e \delta_e δe 正对的边长为 k e v f \frac{ke}{v_f} vfke邻边 d 1 d1 d1这样就可求得斜边为 ( 1 ( k e ( t ) v f ) 2 ) \sqrt{\left( 1\left( \frac{ke\left( t \right)}{v_f} \right) ^2 \right)} (1(vfke(t))2) 所以 δ e k e v f ( 1 ( k e ( t ) v f ) 2 \delta _e\frac{ke}{v_f\sqrt{\text{(}1\left( \frac{ke\left( t \right)}{v_f} \right) ^2}} δevf(1(vfke(t))2 ke把这一项代回上面的公式可以把 v f v_f vf 消掉得到横向误差变化率 e ˙ ( t ) − k e ( t ) ( 1 ( k e ( t ) v f ) 2 ) \dot e(t) \frac { - k e ( t ) } { \sqrt { ( 1 ( \frac { k e ( t ) } { v _ { f } } ) ^ { 2 } ) } } e˙(t)(1(vfke(t))2) −ke(t)
假设横向误差相对较小则分母部分可近似为 1 1 1可以得到微分方程 e ˙ ( t ) ≈ − k e ( t ) \dot { e } ( t ) \approx - k e ( t ) e˙(t)≈−ke(t) 跟踪误差的解为自然指数衰减形式自然指数衰减的方程就是稳定的同时根据微分方程还可以看出当 k k k 值增加时衰减更快就可以加快误差纠正。
5.2 相轨迹图 可通过对闭环系统的相轨迹图来分析 也可以看出方程的运动轨迹在一定范围之内有界所以稳定。 六、Stanley 算法优化 在实际运用中Stanley 算法仅仅是基于几何模型的轨迹追踪控制器因此并没有考虑到实际车辆很多不同的方面比如噪声的测试、执行器的动力学轮胎力效应等等而所有的这些因素都可能在跟踪轨迹过程中导致不良的特性但可以在对 Stanley 算法做一些调整来避免不良的问题。
6.1 低速情况下的噪声软化 在低速情况下Stanley 算法面对很大噪声时对速度的估计会表现得非常激进由于此处速度这一项在反正切的分母下面 δ ( t ) θ e ( t ) tan − 1 ( k e ( t ) v ( t ) ) \delta ( t ) \theta _ { e } ( t ) \tan ^ { - 1 } ( \frac { k e ( t ) } { v ( t ) } ) δ(t)θe(t)tan−1(v(t)ke(t)) 在低速估计中如果有一点误差 Δ v \Delta v Δv会导致整个转向命令被放大从而导致转向角度出现剧烈摆动。 这一点对车辆驾驶的舒适度影响非常大举一个非常极端的例子假如在高速行驶速度约为 20 m / s 20m/s 20m/s 时若速度估计中存在噪声 Δ v 2 m / s \Delta v2m/s Δv2m/s 当然这是非常夸大的噪声。在正常高速行驶下此噪声只占 10 10% 10其实没什么关系。 但如果把噪声放到泊车场景中可能实际场景的行驶速度只有 3 − 5 m / s 3 - 5m/s 3−5m/s这时加了这么大的噪声可能就占 50 50 50% 以上对转向角造成很大影响。 当然这是非常极端的例子在现实生活中很难出现这么大的噪声但我们就是要夸大的讲问题。 所以这时候要怎么解决问题 为了摆脱噪声问题来提高低速的行驶稳定性常常会加入软化常数 k s k_s ks δ ( t ) θ e ( t ) tan − 1 ( k e ( t ) k s v f ( t ) ) \delta ( t ) \theta _ { e } ( t ) \tan ^ { - 1 } \left( \frac { k e ( t ) } { k _ { s } v _ { f } ( t ) } \right) δ(t)θe(t)tan−1(ksvf(t)ke(t))其中 k s 0 k_s0 ks0。 这样就能够确保分母始终具有最小值 k s k_s ks 可根据实际实验场景调整这样就保证了在低速行驶的稳定性。
6.2 高速情况下的航向阻尼 在高速情况下当有转向命令时需要转向角更缓慢的变化以确保侧向力不会过大。而即使在 Stanley 算法里已经有速度的考量引入了速度量 v f v_f vf但 Stanley 算法在高速时反应还是有些过于激进。因此常常会在车头朝向即 δ θ e δ e \delta \theta _ { e } \delta _ { e } δθeδe 在航向角误差上可以加入阻尼项实质上就是在航向角误差的控制部分加了 P D PD PD 控制器这样就可以确保在高速时转向指令可以更缓慢、更准确得确保侧向力不会过大。
6.3 引入前馈项 因为 Stanley 算法和 Pure Pursuit 算法都是根据几何模型推导的控制器并没有考虑参考路径的曲率。 所以可通过在航向上引入前馈项来改善曲线跟踪性能一般前馈项只需要和路径曲率保持一致即可即只需增加和路径曲率相同的转向角。
6.4 综合优化 在实际应用中横向控制可以转化为 P D PD PD 控制器加入软化常数 k s k_s ks再加上前馈控制项 δ f f ( κ ) \delta _{ff}( \kappa ) δff(κ) δ ′ P D [ θ e ] arctan ( k e k s v f ) δ f f ( κ ) \delta PD\left[ \theta _e \right] \arctan \left( \frac{ke}{k_sv_f} \right) \delta _{ff}\left( \kappa \right) δ′PD[θe]arctan(ksvfke)δff(κ) 前馈控制项 δ f f ( κ ) \delta _{ff}( \kappa ) δff(κ) 要和实际路径曲率 κ \kappa κ 直接关联通过这些优化措施就可以使 Stanley 控制器成为更舒适、更准确的轨迹跟踪控制器。 七、Stanley 算法的控制直观性
7.1 直接调整航向角误差和横向误差 Stanley 算法比 Pure Pursuit 调节起来更直观因为控制律直接就划分好一部分矫正航向误差一部分矫正横向误差。 八、Stanley 算法的调节陷阱 Stanley 算法也会掉入和纯跟踪控制相似的陷阱
8.1 比例系数增大会导致不稳定 当调节比例系数 k k k 时可能会增大 k k k 刚开始会使跟踪性能变好但增大到一定程度后控制器就变得不稳定。
8.2 避免特定场景的过度调节 不建议 Stanley 算法对某个特定场景进行过度调节。对于不同的场景比如直线调的非常好可能对曲线就不能很好地适应。相同的当对曲线进行很好的调节时可能对与直线轨迹的性能就不是特别好了。 因为毕竟和纯跟踪算法一样是基于几何模型的控制在控制律中并没有体现与参考轨迹曲率的关系。
8.3 与纯跟踪算法相比的转弯过冲现象 与 Pure Pursuit 纯跟踪算法不同的是Stanley 控制器没有前视机制无法预测前方路况因此在转弯时只能根据当前状态进行控制容易出现转弯过冲现象。 另外与 Pure Pursuit 纯跟踪算法相似的地方就是对速度增加时曲线的稳态误差会变得很大都会影响跟踪准确性主要因为算法没有体现与参考轨迹曲率的关系一般可通过加前馈项来提高稳态误差。 九、编程实现
9.1 Python 实现
Path tracking simulation with Stanley steering control and PID speed control.author: Atsushi Sakai (Atsushi_twi)
import sys
sys.path.append(../../PathPlanning/CubicSpline/)import math
import matplotlib.pyplot as plt
import cubic_spline_plannerk 0.5 # control gain
Kp 1.0 # speed propotional gain
dt 0.1 # [s] time difference
L 2.9 # [m] Wheel base of vehicle
max_steer math.radians(30.0) # [rad] max steering angleshow_animation Trueclass State:def __init__(self, x0.0, y0.0, yaw0.0, v0.0):self.x xself.y yself.yaw yawself.v vdef update(state, a, delta):if delta max_steer:delta max_steerelif delta -max_steer:delta -max_steerstate.x state.x state.v * math.cos(state.yaw) * dtstate.y state.y state.v * math.sin(state.yaw) * dtstate.yaw state.yaw state.v / L * math.tan(delta) * dtstate.yaw pi_2_pi(state.yaw)state.v state.v a * dtreturn statedef PIDControl(target, current):a Kp * (target - current)return adef stanley_control(state, cx, cy, cyaw, pind):ind, efa calc_target_index(state, cx, cy)if pind ind:ind pindtheta_e pi_2_pi(cyaw[ind] - state.yaw)theta_d math.atan2(k * efa, state.v)delta theta_e theta_dreturn delta, inddef pi_2_pi(angle):while (angle math.pi):angle angle - 2.0 * math.piwhile (angle -math.pi):angle angle 2.0 * math.pireturn angledef calc_target_index(state, cx, cy):# calc frant axle positionfx state.x L * math.cos(state.yaw)fy state.y L * math.sin(state.yaw)# search nearest point indexdx [fx - icx for icx in cx]dy [fy - icy for icy in cy]d [math.sqrt(idx ** 2 idy ** 2) for (idx, idy) in zip(dx, dy)]mind min(d)ind d.index(mind)tyaw pi_2_pi(math.atan2(fy - cy[ind], fx - cx[ind]) - state.yaw)if tyaw 0.0:mind - mindreturn ind, minddef main():# target courseax [0.0, 100.0, 100.0, 50.0, 60.0]ay [0.0, 0.0, -30.0, -20.0, 0.0]cx, cy, cyaw, ck, s cubic_spline_planner.calc_spline_course(ax, ay, ds0.1)target_speed 30.0 / 3.6 # [m/s]T 100.0 # max simulation time# initial statestate State(x-0.0, y5.0, yawmath.radians(20.0), v0.0)lastIndex len(cx) - 1time 0.0x [state.x]y [state.y]yaw [state.yaw]v [state.v]t [0.0]target_ind, mind calc_target_index(state, cx, cy)while T time and lastIndex target_ind:ai PIDControl(target_speed, state.v)di, target_ind stanley_control(state, cx, cy, cyaw, target_ind)state update(state, ai, di)time time dtx.append(state.x)y.append(state.y)yaw.append(state.yaw)v.append(state.v)t.append(time)if show_animation:plt.cla()plt.plot(cx, cy, .r, labelcourse)plt.plot(x, y, -b, labeltrajectory)plt.plot(cx[target_ind], cy[target_ind], xg, labeltarget)plt.axis(equal)plt.grid(True)plt.title(Speed[km/h]: str(state.v * 3.6)[:4])plt.pause(0.001)# Testassert lastIndex target_ind, Cannot goalif show_animation:plt.plot(cx, cy, .r, labelcourse)plt.plot(x, y, -b, labeltrajectory)plt.legend()plt.xlabel(x[m])plt.ylabel(y[m])plt.axis(equal)plt.grid(True)flg, ax plt.subplots(1)plt.plot(t, [iv * 3.6 for iv in v], -r)plt.xlabel(Time[s])plt.ylabel(Speed[km/h])plt.grid(True)plt.show()if __name__ __main__:main()9.2 Matlab 实现
clc
clear allk 0.1; % look forward gain
global Kp ;
Kp 1.0 ; % speed propotional gain
dt 0.1 ;% [s]
L 2 ;% [m] wheel base of vehicle
cx 0:0.1:200; % sampling interception from 0 to 100, with step 0.1
for i 1:500% here we create a original reference line, which the vehicle should always follow when there is no obstacles;cy(i) -sin(cx(i)/10)*cx(i)/8;
end
for i 501: length(cx)cy(i) -sin(cx(i)/10)*cx(i)/8; %cy(500);
end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% here we provide another reference line for testing % cx [0.0, 0.0820204315351055, 0.16405076165331595, 0.24610088893773643, 0.32818071197147175, 0.41030012933762705, 0.4924690396193074, 0.5746973413996174, 0.6569949332616624, 0.7393717137885472, 0.8218375815633768, 0.9044024351692563, 0.9870761731892908, 1.0698686942065845, 1.1527898968042436, 1.235849679565372, 1.3190579410730754, 1.4024245799104587, 1.485959494660626, 1.5696725839066838, 1.6535737462317357, 1.7376728802188874, 1.821979884451244, 1.9065046575119102, 1.9912570979839908, 2.076247104450591, 2.161484575494815, 2.24697940969977, 2.3327415056485585, 2.4187807619242876, 2.5051070771100594, 2.5917303497889823, 2.678660478544159, 2.7659073619586954, 2.853480898615696, 2.9413909870982655, 3.0296475259895104, 3.1182604138725343, 3.2072395493304424, 3.29659483094634, 3.3863361573033313, 3.476473426984523, 3.5670165385730184, 3.6579753906519223, 3.749359881804342, 3.84117991061338, 3.9334453756621435, 4.026166175533734, 4.119352208811261, 4.213013374077826, 4.307159569916534, 4.401800694910493, 4.496946647642805, 4.5926073266965775, 4.6887926306549135, 4.785512458100917, 4.882776707617697, 4.980595277788353, 5.0789780671959965, 5.177934974423726, 5.277475898054651, 5.377610736671875, 5.478349388858503, 5.579701753197639, 5.68167772827239, 5.784287212665859, 5.887540104961153, 5.991446303741374, 6.096009197990546, 6.201198840432942, 6.306974641055681, 6.413296005199144, 6.520122338203711, 6.627413045409761, 6.735127532157672, 6.843225203787827, 6.9516654656406045, 7.060407723056383, 7.169411381375547, 7.278635845938471, 7.388040522085536, 7.497584815157124, 7.607228130493614, 7.716929873435386, 7.826649449322817, 7.936346263496289, 8.045979721296185, 8.155509228062881, 8.264894189136756, 8.374094009858192, 8.483068095567571, 8.591775851605268, 8.700176683311668, 8.808229996027144, 8.915895195092082, 9.02313168584686, 9.129898873631856, 9.236156163787452, 9.341862961654027, 9.446978672571962, 9.551462701881636, 9.655274454923427, 9.758373337037717, 9.860718753564889, 9.962270109845317, 10.06298681121938, 10.162828263027466, 10.261753870609947, 10.359723039307207, 10.456695174459625, 10.552629681407577, 10.647485965491452, 10.741223432051621, 10.833801486428465, 10.92517953396237, 11.015316979993708, 11.104173229862862, 11.191707688910215, 11.277879762476145, 11.362648855901028, 11.44597437452525, 11.527815723689187, 11.608132308733218, 11.686883534997726, 11.764028807823086, 11.839527532549685, 11.9133391145179, 11.985422959068107, 12.05573847154069, 12.124245057276028, 12.1909021216145, 12.255669069896483, 12.318505307462365, 12.379370239652518, 12.438223271807324, 12.495023809267167, 12.54973890452178, 12.602375656449803, 12.652954228541832, 12.7014947915228, 12.74801751611764, 12.792542573051287, 12.835090133048672, 12.875680366834732, 12.914333445134396, 12.951069538672604, 12.985908818174284, 13.018871454364373, 13.049977617967805, 13.079247479709512, 13.106701210314425, 13.132358980507481, 13.156240961013616, 13.178367322557756, 13.198758235864842, 13.217433871659805, 13.234414400667582, 13.249719993613098, 13.263370821221296, 13.275387054217104, 13.28578886332546, 13.294596419271288, 13.301829892779535, 13.307509454575126, 13.311655275382996, 13.314287525928084, 13.315426376935315, 13.315091999129628, 13.313304563235954, 13.31008423997923, 13.305451200084386, 13.299425614276359, 13.29202765328008, 13.283277487820486, 13.273195288622507, 13.26180122641108, 13.249115471911132, 13.235158195847605, 13.219949568945427, 13.203509761929535, 13.185858945524863, 13.167017290456341, 13.147004967448904, 13.125842147227486, 13.10354900051702, 13.080145698042445, 13.055652410528687, 13.030089308700683, 13.003476563283368, 12.975834345001672, 12.947182824580533, 12.91754217274488, 12.886932560219652, 12.855374157729777, 12.822887136000192, 12.78949166575583, 12.755207917721627, 12.720056062622511, 12.684056271183422, 12.647228714129286, 12.609593562185047, 12.571170986075629, 12.531981156525973, 12.492044244261002, 12.451380420005666, 12.410009854484889, 12.367952718423599, 12.325229182546735, 12.281859417579238, 12.237863594246035, 12.193261883272053, 12.148074455382238, 12.102321481301514, 12.056023131754824, 12.00919957746709, 11.961870989163257, 11.914057537568251, 11.865779393407012, 11.817056727404465, 11.767909710285553, 11.718358512775206, 11.668423305598353, 11.618124259479929, 11.567481545144876, 11.516515333318122, 11.465245794724597, 11.41369310008924, 11.361877420136981, 11.309818925592758, 11.2575377871815, 11.205054175628145, 11.152388261657622, 11.099560215994869, 11.046590209364814, 10.9934984124924, 10.940304996102551, 10.887030130920206, 10.833693987670296, 10.780316737077758, 10.726918549867523, 10.673519596764525, 10.620140048493699, 10.566800075779973, 10.51351984934829, 10.460319539923576, 10.40721931823077, 10.354239354994803, 10.301399820940603, 10.248720886793116, 10.19622272327727, 10.143925501117993, 10.091849391040228, 10.0400145637689, 9.98844107489259, 9.937130775190631, 9.886047589576616, 9.835150704129385, 9.784399304927794, 9.7337525780507, 9.68316970957695, 9.632609885585403, 9.582032292154906, 9.531396115364313, 9.480660541292481, 9.42978475601826, 9.378727945620502, 9.327449296178056, 9.275907993769783, 9.224063224474532, 9.171874174371155, 9.119300029538504, 9.066299976055433, 9.012833200000797, 8.958858887453443, 8.90433622449223, 8.849224397196009, 8.79348259164363, 8.737069993913945, 8.679945790085814, 8.622069166238083, 8.563399308449608, 8.50389540279924, 8.44351663536583, 8.382222192228236, 8.319971259465307, 8.256723023155896, 8.192436669378857, 8.127071384213043, 8.060586353737307, 7.992940764030496, 7.924093801171472, 7.854004651239081, 7.782632500312176, 7.709936534469614, 7.635875939790246, 7.5604099023529265, 7.483497762957981, 7.40512683270666, 7.325344607833358, 7.244206394660972, 7.1617674995124165, 7.078083228710588, 6.993208888578398, 6.907199785438752, 6.820111225614551, 6.731998515428706, 6.642916961204114, 6.552921869263686, 6.4620685459303315, 6.370412297526947, 6.278008430376444, 6.184912250801722, 6.091179065125691, 5.996864179671257, 5.902022900761322, 5.806710534718794, 5.710982387866576, 5.614893766527573, 5.518499977024695, 5.421856325680841, 5.325018118818923, 5.228040662761838, 5.130979263832497, 5.033889228353805, 4.9368258626486705, 4.8398444730399905, 4.743000365850674, 4.646348847403626, 4.549945224021753, 4.453844802027968, 4.358102887745161, 4.262774787496245, 4.167915807604124, 4.073581254391704, 3.9798264341818985, 3.8867066532975985, 3.7942772180617155, 3.702593434797154, 3.61171060982682, 3.5216840494736275, 3.432569060060466, 3.344420947910249, 3.2572950193458796, 3.171246580690265, 3.086330938266317, 3.0026033983969267, 2.920108549621215, 2.8388438819014783, 2.7587939515191384, 2.6799433143929905, 2.6022765264418104, 2.5257781435843945, 2.450432721739531, 2.3762248168260083, 2.3031389847626214, 2.2311597814681488, 2.160271762861384, 2.0904594848611158, 2.021707503386134, 1.9540003743552317, 1.8873226536871883, 1.8216588973007979, 1.7569936611148487, 1.69331150104813, 1.6305969730194354, 1.5688346329475444, 1.508009036751251, 1.4481047403493437, 1.3891062996606112, 1.3309982706038468, 1.2737652090978315, 1.2173916710613577, 1.161862212413215, 1.1071613890721919, 1.053273756957081, 1.0001838719866638, 0.947876290079733, 0.8963355671550775, 0.8455462591314863, 0.795492921927752, 0.7461601114626558, 0.6975323836549908, 0.6495942944235455, 0.6023303996871092, 0.5557252553644707, 0.5097634173744222, 0.4644294416357458, 0.41970788406723397, 0.3755833005876753, 0.33204024711585906, 0.28906327957057754, 0.24663695387061274, 0.204745825934757, 0.16337445168179898, 0.12250738703052838, 0.08212918789973561, 0.04222441020820539, 0.0027776098747295253, -0.03622665718190471, -0.07480383504290766, -0.1129693677894879, -0.15073869950286078, -0.1881272742642346, -0.22515053615482283, -0.2618239292558341, -0.29816289764847836, -0.33418288541397156, -0.3698993366335206, -0.40532769538833924, -0.4404834057596364, -0.47538191182862255, -0.5100386576765124, -0.5444690873845134, -0.5786886450338401, -0.6127127747057002, -0.6465569204813051, -0.6802365264418688, -0.7137670366685996, -0.7471638952427104, -0.780442546245411, -0.8136184337579115, -0.846707001861426, -0.8797236946371636, -0.9126839561663362, -0.9456032305301545, -0.9784969618098274]
% cy [0.0, -0.037366703007607714, -0.07474321440358046, -0.11213934257628329, -0.1495648959140812, -0.1870296828053393, -0.22454351163842262, -0.2621161908016961, -0.29975752868352495, -0.3374773336722741, -0.37528541415630867, -0.41319157852399363, -0.45120563516369416, -0.48933739246377506, -0.5275966588126015, -0.5659932425985387, -0.6045369522099514, -0.6432375960352048, -0.6821049824626639, -0.721148919880694, -0.7603792166776596, -0.7998056812419262, -0.8394381219618586, -0.8792863472258223, -0.9193601654221818, -0.959669384939302, -1.0002238141655486, -1.0410332614892863, -1.08210753529888, -1.1234564439826953, -1.1650897959290965, -1.207017399526449, -1.2492490631631181, -1.2917945952274683, -1.3346638041078651, -1.3778664981926732, -1.4214124858702577, -1.465311575528984, -1.5095735755572173, -1.5542082943433218, -1.5992255402756625, -1.6446351217426058, -1.6904468471325154, -1.7366705248337566, -1.7833159632346953, -1.8303929707236954, -1.877911355689123, -1.9258809265193417, -1.9743114916027187, -2.0232128593276166, -2.0725948380824017, -2.12246723625544, -2.1728398622350946, -2.2237225244097316, -2.2751250311677156, -2.3270571908974125, -2.379528811987187, -2.432549702825403, -2.4861296718004278, -2.5402785273006243, -2.595006077714358, -2.650322131429995, -2.7062364968358987, -2.7627589823204364, -2.8198993962719707, -2.877667547078868, -2.936073243129494, -2.9951262928122118, -3.054826476456499, -3.115122219773589, -3.175945553302979, -3.2372285004258474, -3.2989030845233707, -3.36090132897673, -3.4231552571671022, -3.4855968924756664, -3.5481582582836015, -3.6107713779720854, -3.6733682749222965, -3.735880972515414, -3.798241494132616, -3.8603818631550806, -3.9222341029639893, -3.9837302369405156, -4.044802288465841, -4.105382280921146, -4.165402237687605, -4.2247941821464, -4.283490137678707, -4.341422127665705, -4.398522175488574, -4.454722304528492, -4.509954538166638, -4.564150899784188, -4.617243412762323, -4.66916410048222, -4.719844986325061, -4.76921809367202, -4.817215445904277, -4.863769066403011, -4.908810978549401, -4.952273205724626, -4.994087771309864, -5.034186698686292, -5.072502011235089, -5.1089657323374364, -5.1435098853745105, -5.17606649372749, -5.206567580777552, -5.2349451699058775, -5.261131284493644, -5.28505794792203, -5.306657183572213, -5.325861014825374, -5.342601465062691, -5.35681055766534, -5.368420316014503, -5.377362763491355, -5.3835699234770775, -5.386973819352848, -5.387506474499846, -5.385099912299248, -5.379686156132234, -5.37119722937998, -5.35956515542367, -5.344721957644477, -5.326599659423584, -5.305130284142165, -5.280245855181402, -5.251878395922473, -5.219959929746556, -5.1844224800348275, -5.145198070168469, -5.102218723528661, -5.0554164634965755, -5.004723313453395, -4.950090134033602, -4.891566432300125, -4.829233897426258, -4.763174236405681, -4.693469156232071, -4.620200363899109, -4.543449566400473, -4.4632984707298435, -4.379828783880898, -4.293122212847316, -4.203260464622775, -4.110325246200959, -4.014398264575541, -3.9155612267402033, -3.813895839688624, -3.7094838104144814, -3.602406845911456, -3.492746653173226, -3.3805849391934713, -3.26600341096587, -3.1490837754840997, -3.029907739741842, -2.908557010732775, -2.7851132954505786, -2.65965830088893, -2.5322737340415067, -2.4030413019019954, -2.272042711464064, -2.139359669721398, -2.0050738836676802, -1.8692670602965795, -1.7320209066017864, -1.5934171295769686, -1.4535374362158098, -1.3124635335119952, -1.170277128459193, -1.0270599280510933, -0.8828936392813632, -0.7378599691436885, -0.5920406246317518, -0.4455173127392231, -0.298371740459791, -0.15068561478712494, -0.002540642714907948, 0.14598146876317464, 0.29479901265345454, 0.4438302819622424, 0.5929935696958706, 0.7422071688606537, 0.8913893724629078, 1.0404584735089641, 1.1893327650051344, 1.337930539957748, 1.4861700913731224, 1.633969712257573, 1.78124769561743, 1.9279223344590108, 2.0739119217886306, 2.219134750612622, 2.363509113937294, 2.5069533047689756, 2.649385616113989, 2.7907243409786435, 2.9308877723692763, 3.0697942032921937, 3.2073619267537268, 3.3435092357601945, 3.478154423317913, 3.6112157824332094, 3.7426116061123986, 3.8722601873618068, 4.000079819187756, 4.125988794596558, 4.249905406594547, 4.3717479481880295, 4.491434712383341, 4.608883992186794, 4.724014080604706, 4.836743270643411, 4.946989855309216, 5.054672127608452, 5.159708380547434, 5.262016907132485, 5.361516000369923, 5.458123953266078, 5.551759058827262, 5.642339610059803, 5.729783899970013, 5.814010221564217, 5.89493686784874, 5.972482131829901, 6.0465643065140195, 6.1171016849074125, 6.184012560016411, 6.247215224847325, 6.306627972406487, 6.362169095700208, 6.413756887734815, 6.4613096415166265, 6.504745650051962, 6.543983206347152, 6.578940603408499, 6.60953613424234, 6.635688091854995, 6.6573147692527765, 6.6743344594420115, 6.686665455429019, 6.6942260502201165, 6.6969345368216295, 6.694709208239885, 6.68746835748119, 6.6751302775518795, 6.657613261458263, 6.634835602206671, 6.606715592803418, 6.573171526254818, 6.534121695567208, 6.48948507667127, 6.439288626224071, 6.383784255864741, 6.323251985344354, 6.257971834413989, 6.188223822824728, 6.114287970327644, 6.03644429667382, 5.95497282161433, 5.870153564900254, 5.782266546282674, 5.691591785512662, 5.598409302341302, 5.502999116519667, 5.405641247798836, 5.306615715929894, 5.20620254066391, 5.10468174175197, 5.002333338945146, 4.89943735199452, 4.796273800651171, 4.693122704666173, 4.590264083790611, 4.487977957775556, 4.386544346372089, 4.286243269331292, 4.187354746404237, 4.090158797342008, 3.9949354418956777, 3.901964699816327, 3.811526590855039, 3.723901134762883, 3.639368351290946, 3.5582082601902973, 3.480700881212022, 3.407126234107199, 3.3377643386269, 3.27289521452221, 3.2127988815442015, 3.1577553594439567, 3.1080446679725546, 3.063946826881071, 3.0257418559205838, 2.9937089792748877, 2.9679835997506907, 2.948391652310318, 2.9347189129743403, 2.926751157763331, 2.924274162697862, 2.9270737037985057, 2.934935557085835, 2.947645498580423, 2.9649893043028404, 2.986752750273661, 3.0127216125134586, 3.0426816670428023, 3.0764186898822676, 3.1137184570524243, 3.1543667445738492, 3.198149328467111, 3.244851984752782, 3.294260489451438, 3.346160618583647, 3.4003381481699866, 3.4565788542310263, 3.514668512787337, 3.574392899859495, 3.635537791468069, 3.697888963633636, 3.761232192376766, 3.8253532537180313, 3.8900379236780003, 3.955071978277253, 4.020241193536361, 4.085331345475892, 4.150128210116423, 4.214417563478519, 4.277985181582762, 4.340616840449722, 4.402098316099968, 4.462215384554076, 4.520753821832612, 4.577499403956158, 4.6322379069452815, 4.684755106820557, 4.734836779602553, 4.782268701311841, 4.826836647969001, 4.868326395594604, 4.906523720209217, 4.941214397833415, 4.972184204487769, 4.999218916192856, 5.022144570218274, 5.0409641291725205, 5.055729140908652, 5.066491154641944, 5.073301719587677, 5.076212384961124, 5.075274699977566, 5.0705402138522775, 5.062060475800537, 5.049887035037623, 5.034071440778809, 5.014665242239377, 4.991719988634599, 4.965287229179759, 4.935418513090126, 4.902165389580982, 4.865579407867605, 4.825712117165269, 4.7826150666892575, 4.73633980565484, 4.686937883277296, 4.634460848771905, 4.5789602513539425, 4.520487640238691, 4.4590945646414175, 4.394832573777406, 4.327753216861931, 4.257908043110272, 4.18534860173771, 4.110126441959512, 4.032293112990962, 3.951900164047335, 3.86899914434391, 3.7836416030959685, 3.695879089518778, 3.605763152827619, 3.5133453422377716, 3.4186772069645106, 3.3218102962231137, 3.222796159228867, 3.1216863451970314, 3.0185324033428924, 2.9133858828817267, 2.8062983330288125, 2.697321302999434, 2.5865063420088523, 2.4739049992723525, 2.359568824005213, 2.2435493654227114, 2.125898172740131, 2.006666795172734, 1.8859067819358044, 1.7636696822446223, 1.6400070453144635, 1.5149704203606134, 1.3886113565983302, 1.2609814032429025, 1.1321321095096089, 1.0021150246137207, 0.8709816977705314, 0.7387836781952948, 0.6055725151032996, 0.4713997577098228, 0.3363169552301395, 0.20037565687954162, 0.06362741187328114, -0.07387623057335091, -0.21208372124507902, -0.35094351092662635, -0.4904040504027045, -0.6304137904580571, -0.7709211818773953, -0.9118746754454454, -1.0532227219469266, -1.194913772166554, -1.336896276889068, -1.4791186868991812, -1.62152945298162, -1.7640770259211065, -1.9067098565023501]% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%p [cx, cy];%计算一阶导数for i 1:length(cx)-1pd(i) (p(i1,2)-p(i,2))/(p(i1,1)-p(i,1));endpd(end1) pd(end);%计算二阶导数for i 2: length(cx)-1pdd(i) (p(i1,2)-2*p(i,2) p(i-1,2))/(0.5*(-p(i-1,1)p(i1,1)))^2;endpdd(1) pdd(2);pdd(length(cx)) pdd(length(cx)-1);%计算曲率 for i 1:length(cx)-1k(i) (pdd(i))/(1pd(i)^2)^(1.5);endcyaw atan(pd);
% cyaw [-0.42747397147825394, -0.42750072065132655, -0.42758094489540044, -0.4277145744333646, -0.42790149313092396, -0.42814153873731675, -0.4284345032210795, -0.42878013319963476, -0.4291781304611309, -0.42962815257663806, -0.430129813600478, -0.4306826848561635, -0.4312862958051237, -0.4319401349951135, -0.43264365108494557, -0.4333962539419319, -0.4341973158082054, -0.4350461725318764, -0.4359421248588016, -0.4368844397805786, -0.43787235193423374, -0.43890506504896076, -0.4399817534351663, -0.4411015635110099, -0.4422636153615753, -0.4434670043257879, -0.44471080260618273, -0.4459940608966561, -0.4473158100233627, -0.4486750625939931, -0.4500708146507374, -0.4515020473223469, -0.452967728470822, -0.4544668143283879, -0.4559982511205717, -0.457560976671358, -0.45915392198657834, -0.46077601281187724, -0.4624261711617995, -0.46410331681674805, -0.46580636878477943, -0.4675342467254242, -0.4692858723329443, -0.4710601706766694, -0.47285607149628356, -0.4746725104501654, -0.47650843031511225, -0.4783627821360113, -0.4802345263242414, -0.4821226337038107, -0.484026086504453, -0.48594387930111194, -0.48787501989944665, -0.4898185301671866, -0.49177344681135127, -0.4937388221015239, -0.4957137245395447, -0.49769723947613925, -0.49968846967515207, -0.5016865358261939, -0.503690577006636, -0.5056997510940054, -0.5077132351299432, -0.5097302256369819, -0.511749938889487, -0.5137716111401838, -0.5157944988037568, -0.5178178785990676, -0.5196931149270214, -0.5212243260431461, -0.5224197163289975, -0.5232873261735816, -0.5238339185427856, -0.524065048216695, -0.5239851199517127, -0.5235974365070971, -0.5229042373201364, -0.5219067284762255, -0.5206051044967731, -0.5189985623566975, -0.5170853080421537, -0.5148625558662738, -0.5123265206745073, -0.5094724029903408, -0.5062943670756251, -0.5027855118065777, -0.49893783419608717, -0.4947421853247934, -0.49018821837741167, -0.4852643284170402, -0.47995758346934314, -0.47425364643153906, -0.46813668726977986, -0.46158928492527146, -0.4545923183179331, -0.4471248458215146, -0.43916397259260614, -0.43068470517701307, -0.42165979290260597, -0.41205955571398767, -0.4018516983320383, -0.3910011109578644, -0.3794696572209753, -0.3672159507406615, -0.3541951225846469, -0.34035858314180506, -0.3256537835649078, -0.31002398409323995, -0.293408039362483, -0.2757402143998198, -0.2569500495510399, -0.23696229826460727, -0.2156969686215963, -0.19306950785625335, -0.16899117885754447, -0.14336968857788476, -0.11611013987570126, -0.08711638955650051, -0.0562929045043543, -0.02354721209741704, 0.011206963321640584, 0.048045804465186234, 0.08703071058155766, 0.1282028423989582, 0.1715770293292805, 0.2171352954827804, 0.2648204174206136, 0.3145300919358443, 0.36611243518678976, 0.4193636116084794, 0.47402835394583015, 0.5298039462969809, 0.5863478911541079, 0.643289005215774, 0.7002411717905076, 0.7568185361252836, 0.8118690502380587, 0.8641084724045175, 0.9134969015780486, 0.960059587025011, 1.0038662856881957, 1.0450193867705637, 1.0836435702751392, 1.1198772452847279, 1.1538657621615147, 1.1857562421823056, 1.2156937951233464, 1.2438188751256842, 1.27026553679441, 1.2951603814332422, 1.3186220173456338, 1.3407608921349043, 1.3616793857102756, 1.381472078925828, 1.4002261342177875, 1.418021741613346, 1.4349325966649613, 1.4510263868852697, 1.4663652707516057, 1.481006338868105, 1.4950020508771902, 1.5084006445706022, 1.5212465156536303, 1.5335805679889383, 1.545440535058924, 1.5568612739662109, 1.5678750336361222, 1.5785116990634436, 1.5887990135100851, 1.5987627805478712, 1.6084270477787184, 1.6178142739720436, 1.6269454812500126, 1.635840393834563, 1.6445175647523944, 1.6529944917796306, 1.6612877237992376, 1.6694129586432487, 1.6773851333992504, 1.685218508076781, 1.6929267434543929, 1.7005229738619254, 1.7080198755947573, 1.7154297316071752, 1.7227644930901023, 1.7300358385040377, 1.7372552306108673, 1.744433972028028, 1.751583259815283, 1.7587142395979847, 1.7658380597313836, 1.7729659260183661, 1.7801091575084258, 1.7872792439290976, 1.7944879053332161, 1.801747154587014, 1.8090693633763313, 1.8164673324723377, 1.8239543670758143, 1.8315443581520676, 1.8392518707793255, 1.8470922406647414, 1.855081680137207, 1.8632373951090009, 1.8715777147135213, 1.8801222355794698, 1.8888919829993627, 1.8979095915996806, 1.9071995085302154, 1.9167882226713098, 1.9267045239213823, 1.936979797286335, 1.9476483572613554, 1.9587478288892548, 1.9703195829126208, 1.9824092336227193, 1.9950672093546409, 2.0083494070846104, 2.0223179442338215, 2.0370420225265327, 2.052598920493523, 2.0690751327826553, 2.086567675540389, 2.1051855772731987, 2.125051572999925, 2.1463040149253114, 2.1690990033792454, 2.1936127244444314, 2.2200439511487113, 2.2486166168989628, 2.2795822938387658, 2.3132222925563584, 2.3498489271506036, 2.3898052430761987, 2.433462168992391, 2.4812116264501998, 2.5334536477941385, 2.590575124560417, 2.6529176801580223, 2.7207327634867218, 2.794124005772281, 2.8729807692041156, 2.9569127030418287, 3.0452016965400053, 3.136791572382236, -3.0528526363431867, -2.9588998772229815, -2.866118078467373, -2.775975743675515, -2.689697060846188, -2.608184767719592, -2.532002337116722, -2.4614041798828104, -2.3965048038800147, -2.3397948760409086, -2.291344036566652, -2.249929188652858, -2.2145029415177375, -2.1841871135510753, -2.15825404315402, -2.1361045911182837, -2.117247006290865, -2.1012783168776954, -2.087868670762281, -2.07674849089681, -2.0676981000336987, -2.0605394221252733, -2.0551293927447887, -2.051354762141669, -2.049128030743504, -2.048384309028064, -2.0490789386074084, -2.0511857486905054, -2.0546958525159713, -2.059616912890818, -2.0659728256116554, -2.073803785046152, -2.0831667079739544, -2.0941360000040827, -2.106804653092188, -2.121285661829316, -2.137713738342337, -2.156247287694478, -2.1770705727702664, -2.2003959426511295, -2.226465911379705, -2.2555547413252683, -2.2879689905787797, -2.3240462096482406, -2.364150607796797, -2.4086640629261113, -2.457970378933774, -2.512430353061553, -2.572345306978107, -2.63790775477262, -2.7091404559568084, -2.785693709954596, -2.8629771777058166, -2.9381969335531912, -3.0105047505307554, -3.079236843819787, 3.1392557203024505, 3.0788740583345158, 3.022908226491498, 2.9713293509988272, 2.9240098803318566, 2.880755549938086, 2.841332037315793, 2.805485658729539, 2.772958486701022, 2.743498760169683, 2.7168675954883774, 2.692842944371884, 2.6712215957393837, 2.651819848518777, 2.6344733256498656, 2.6190362691126525, 2.6053805541017656, 2.5933945845903703, 2.582982177799927, 2.574061506732527, 2.566564143762559, 2.5604342309811536, 2.5556277919912787, 2.552112193285384, 2.549865759850427, 2.5488775482819763, 2.5491472807805122, 2.550685444476995, 2.553513562245124, 2.5576646432249412, 2.5631838234373894, 2.570129208795003, 2.5785729340730743, 2.588602451346434, 2.6003220590186795, 2.6138546763318944, 2.6293438557915674, 2.6469560037264426, 2.666882741981156, 2.689343283943847, 2.7145866051993917, 2.7428930491370123, 2.774574803956107, 2.8099744025506865, 2.849460020595651, 2.8920818699119404, 2.936427491384847, 2.982406179787393, 3.02989327954402, 3.0787277146995717, 3.1287126057349637, -3.103567370744916, -3.051999928303144, -3.000050157240052, -2.9480107895802816, -2.8961779597767547, -2.844841379864934, -2.794275039844364, -2.744729158412715, -2.6964239288216736, -2.649545362846886, -2.6042432823839587, -2.560631287824937, -2.5187883747956463, -2.4787617868250718, -2.4405706753745005, -2.4042101741998962, -2.3696555622740223, -2.336866269903388, -2.3057895620788367, -2.276363802585878, -2.2485212577514577, -2.222190439166585, -2.1972980116927356, -2.173770308967378, -2.1515345061653335, -2.1305195014414275, -2.1106565553490393, -2.0918797331909, -2.0741261898580414, -2.057336331021679, -2.041453879044128, -2.0264258669421595, -2.0122025792985068, -1.9987374552102448, -1.9859869651650783, -1.9739104710958324, -1.9624700767129548, -1.9516304734846341, -1.9413587862567534, -1.931624421419231, -1.9223989196776674, -1.9136558148336809, -1.9053704994756007, -1.897520098101366, -1.8900833479118169, -1.8830404873040305, -1.8763731519441875, -1.8700642781943329, -1.8640980135966956, -1.8584596340746704, -1.8531354674846352, -1.8481128231423796, -1.8433799269481883, -1.8389258617424753, -1.834740512537043, -1.8308145162837601, -1.8271392158613986, -1.8237066179815833, -1.8205093547355256, -1.8175406485239551, -1.8147942801330408, -1.8122645597388487, -1.8099463006419043, -1.8078347955515526, -1.805925795257055, -1.8042154895387084, -1.8027004901877028, -1.8013778160180771, -1.8002448797679307, -1.7992994768001862, -1.7985397755256436, -1.7979643094829658, -1.7975719710216365, -1.7973620065449136]ck k;i 1;
target_speed 30/3.6;
T 60;
lastIndex length(cx);
x 0; y 2; yaw 0; v 0;
time 0;
target_ind 0;figurewhile T time target_ind length(cx)[error, target_ind] calc_target_index(x,y,yaw,cx,cy,L)if abs(error) 4fprintf(mayday mayday!\n)break;endai PIDcontrol(target_speed, v,Kp);di stanley_control(x,y,yaw,v,cx,cy,cyaw,target_ind,ck,L, error);[x,y,yaw,v] update(x,y,yaw,v, ai, di,dt,L);time time dt;
% pause(0.1)subplot(3,1,1)plot(cx,cy,b,x,y,r-*,LineWidth,0.2)hold onsubplot(3,1,2)plot( i, error,.b)hold onsubplot(3,1,3)plot( i, yaw,.r)drawnowhold oni i 1;
end
% plot(cx,cy,x,y,*)
% hold onfunction [x, y, yaw, v] update(x, y, yaw, v, a, delta,dt,L)x x v * cos(yaw) * dt;y y v * sin(yaw) * dt;yaw yaw v / L * tan(delta) * dt;v v a * dt;
endfunction [a] PIDcontrol(target_v, current_v, Kp)
a Kp * (target_v - current_v);
endfunction [delta] stanley_control(x,y,yaw,v,cx,cy,cyaw, ind,ck,L, error)tx cx(ind 5);ty cy(ind 5);delta_yaw v * 0.1 * sin(yaw) / L;
% th_e pipi(yaw delta_yaw - cyaw(ind));alpha pipi(atan((ty-y)/(tx-x)) -yaw);gain_k 1;theta atan(gain_k* error/v);delta alpha theta;endfunction [error, ind] calc_target_index(x,y,yaw, cx,cy,L)
x x L * cos(yaw);
y y L * sin(yaw);
N length(cx);
Distance zeros(N,1);
for i 1:N
Distance(i) sqrt((cx(i)-x)^2 (cy(i)-y)^2);
end
[value, location] min(Distance);
ind location;if ycy(ind) error -value;elseerror value;end
endfunction [angle] pipi(angle)if (angle pi)angle angle - 2*pi;
elseif (angle -pi)angle angle 2*pi;
elseangle angle;
end
end十、总结 Stanley 算法是一种基于几何模型的轨迹追踪控制器其设计理念是将参考点设置在前轮中心而非传统的重心、质心或后轮重心。这种设计方式使得该算法能够在路径追踪过程中实现全局收敛并且误差衰减速率不随车辆速度变化而改变从而保证车辆最终收敛到参考轨迹上并且在不同速度下保持良好的稳定性。 Stanley 算法的优势在于控制直观易于理解和调节但同时也存在一些局限性例如转弯过冲、高速误差较大以及对特定场景过度调节可能导致其他场景下性能下降等问题。 为了克服这些局限性可以对 Stanley 算法进行优化例如引入软化常数 、航向阻尼和前馈项等以提高低速行驶的稳定性、确保转向指令缓慢变化并考虑参考路径曲率从而使其成为更舒适、更准确的轨迹跟踪控制器在自动驾驶等领域具有广泛的应用前景。 参考资料 1、自动驾驶规划控制 2、gameinskysky | Github开源项目 3、Stanley轨迹跟踪算法Python/Matlab算法实现 后记 感谢您耐心阅读这篇关于 Stanley 算法详解与编程实现 的技术博客。 如果您觉得这篇博客对您有所帮助请不要吝啬您的点赞和评论 您的支持是我继续创作的动力。同时别忘了收藏本篇博客以便日后随时查阅。 让我们一起期待更多的技术分享共同探索移动机器人的无限可能 感谢您的支持与关注让我们一起在知识的海洋中砥砺前行
