[发明专利]一种基于回旋曲线的自动驾驶车辆路径生成方法

摘要

本发明公开了一种基于回旋曲线的自动驾驶车辆路径生成方法，该方法首先定义了基本回旋曲线对，满足了车辆的运动学特性和轨迹曲率连续特性。之后根据终点的航向角和始终点连线夹角的大小关系将问题分为两类分别解决。两类问题分别采用两段和四段曲线结合的方式完成规划。规划结果具有曲率连续特性，且满足车辆运动学约束。本方法规划的曲线解决了当前路径曲线生成方法中存在的无法满足车辆运动学约束、车辆无法准确跟踪等缺陷，也可以满足换道、转弯等车辆动作，具有较高的实用价值。

主权项

1.一种基于回旋曲线的自动驾驶车辆路径生成方法，其特征在于：该方法的实现步骤如下，步骤1，确定起始点和终点坐标、方向、起点终点时刻的瞬时曲率及车辆航向角，并建立坐标系：设起点为P_i，终点为P_f；以起始点P_i时刻的车辆方向为坐标系Y轴正方向建立坐标系；θ为车辆航向角即车辆航向与X轴夹角，κ为曲率；则起点P_i、终点P_f分别定义为：P_f(x_f,y_f,θ_f,κ_f＝0)；步骤2，定义基本回旋曲线与基本回旋曲线对：步骤2.1，定义基本回旋曲线：由回旋曲线的定义可知，回旋曲线点坐标的x,y应延长度s连续定义；令基本回旋曲线曲率κ(s)为一个从0开始随s变化的一次函数：κ(s)＝αs式中，α为曲率的变化率，为恒定值；因而曲线为曲率连续曲线，保证车辆向心加速度不发生突变；则车辆航向角θ(s)及横纵坐标x(s),y(s)分别定义为：曲线参数在基本回旋曲线的末端点有以下性质：式中，δ为θ从曲线起始端到末端的变化值；将逆时针方向曲率定义为正值；相反，顺时针方向为负值；由以上性质，基本回旋曲线初始曲率为0，并定义δ小于同时回旋曲线需满足车辆运动学约束：式中，v(t)和θ(t)分别是车辆在t时刻的线速度和航向角；x(t)，y(t)表示车辆的位置，即后轮中心位置；L表示车辆轴距；γ(t)为车辆前轮偏角，等于：式中，ρ(t)为转向半径，与曲率半径相等，即为步骤2.2，定义基本回旋曲线对：用满足方向连续性的方式将两只回旋曲线相连接，第二支基本曲线与第一支不同；第一支曲线C₁即P_i至P_m曲率绝对值为从0逐渐增加到κ_m，而第二支曲线C₂即P_m至P_r为从κ_m逐渐减小到0；即两曲线在交点P_m处拥有相同的曲率峰值κ_m；组合后的曲线对记作：在两曲线交点处，曲率相等，且方向相等；故而整对曲线亦满足曲率连续特性；步骤3，根据终点位置规划的航向角θ_f和初始点与终点连线与X轴夹角φ的大小关系，将问题分为两类情况：第一类情况：当θ_f<φ时，两支曲线组合便满足要求；第二类情况：当θ_f>φ时，则需要四支基本回旋曲线组合；当θ_f＝φ时，则只有直线解P_i P_f；步骤4，根据步骤3中的两种两类情况分别迭代求解：第一类，当θ_f<φ时，采用两支基本曲线组合满足要求，迭代算法如下：(1)参数初始化；α₁为第一支基本回旋曲线C₁的曲率变化率；δ₁为第一支基本回旋曲线C₁的航向角变化值，即C₁从P_i到P_m航向角改变了δ₁；dα为每次迭代后α₁的改变值；dδ为每次迭代后δ₁的改变值；ε为算法结束判定条件，即当前回旋曲线对的终点和规划终点P_f位置的判定参数；(2)C₁、C₂曲线生成，并连接为一对；根据步骤2中基本曲线末端曲率κ₁与α₁，δ₁的关系求出C₁末端曲率κ₁；并根据曲率连续特性，C₂曲率最值κ₂＝κ₁；根据曲线几何关系求出C₂曲线转向角变化量δ₂：δ₁+δ₂＝θ_i‑θ_f.式中，θ_i为C₁起始点转向角，即规划起点转向角；θ_f为规划末点转向角；接着，由δ₂，κ₂求出α₂；至此，C₁、C₂曲线所有参数均以求出，并根据步骤2定义过程进行曲线生成，并连接为基本曲线对(3)计算末端点P_r与规划终点P_f距离差值；在规划终点P_f沿航向角θ_f做切线l_f；并同时在规划终点P_f做l_f的垂线而后求出P_r与l_f距离，记作D_e；P_r与距离，记作(4)判断迭代是否停止；判定表达式如下：|D_e|<ε，满足上式，即P_f与P_r垂直终点航向方向的偏差小于阈值ε，同时沿航向的偏差大于或等于0；|D_e|<ε使末端点P_r基本在l_f上，而满足此条件的话，即便也只是再沿l_f方向直线规划一段即可到达P_f；如果满足，即得到此时的α、ε及并结束迭代；若不满足上式条件，则进行以下参数调整计算：λ_pr(pt)＝‑tan(θ_r)·xt+tan(θ_r)·x_r+y_t‑y_r其中，λ_pr(pt)为检查目标点P_t和参照点P_r的几何关系并决定是否对参数进行二分调整的重要参数；同时，由几何关系可知：若满足λ·λ'＜0，则令若满足λ^⊥·λ^⊥'＜0则令即折半迭代；再使：dα＝|dα|·sign(λ)dδ＝|dδ|·sign(λ^⊥)α₁＝(α₁+dα)δ₁＝(δ₁+dδ)λ'＝λλ^⊥'＝λ^⊥之后返回迭代过程(2)，直到迭代结束输出最终结果；第二类，当θ_f>φ时，采用两对共四支基本曲线组合满足要求；每对曲线都有自己的交点P_m1，P_m2；由几何特性，P_m1，P_m2也都是曲率最大的点，因此P_m1，P_m2的切线l_m1，l_m2平行，且与P_i P_f的连线l_f平行；四个基本回旋曲线组合是使用第一类组合的两个连续对实现的；因此，在这两个组合对的交点位置需要确定一个共同的边界条件；做出焦点切线l_m，初始化固定在第一个回旋曲线对的末端，斜率为tanθ_m，其中θ_m是l_m的斜率角；l_m给出了和的方向约束；根据已解决的第一类问题，如果给出共同的方向约束tanθ_m，则将两对回旋曲线分别做为两个第一类回旋问题进行解决。