CN110286671B - 一种基于回旋曲线的自动驾驶车辆路径生成方法 - Google Patents

Abstract

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

Description

一种基于回旋曲线的自动驾驶车辆路径生成方法

技术领域

本发明属于自动驾驶汽车路径规划相关领域，涉及一种多段连续 曲率一阶回旋曲线组合的方法。

背景技术

据公安部统计，截至2018年底，全国一年中新注册登记机动车 3172万辆，机动车保有量已达3.27亿辆，其中汽车2.4亿辆，比2017 年增加2285万辆，增长10.51％，小型载客汽车首次突破2亿辆；机 动车驾驶人突破4亿人，达4.09亿人，其中汽车驾驶人3.69亿人。我国汽车保有量的巨大规模导致了交通事故频频发生。据统计，2018 年全国交通事故约达600万起，其中涉及人员伤亡的交通事故21万 起，死亡人数达6.2万，受伤人数达22.6万人。我国每年交通事故数 量、死亡人数分别约占全国总量的70％和80％，造成直接财产损失约达12亿元。相关研究表明，事故发生前如果有1s的预警时间，90％ 的事故可以得以避免。如果用技术代替人开车，尤其是无人驾驶汽车 与车联网结合，形成一个庞大的移动车联网络，再加上现有的智能交 通系统(Intelligent Transportation System)ITS，则可以大幅提高公路 通行能力，减少公路交通拥堵，减少事故发生。

无人驾驶汽车由环境感知，定位导航，路径规划和运动控制几个 部分组成。其中路径规划模块是指在一定环境模型基础上，给定无人 驾驶汽车起始点与目标点后，按照性能指标规划出一条无碰撞、能安 全到达目标点的有效路径，同时，该路径应能满足车辆的运动学约束 条件以及曲率连续等特性，以使得车辆可以正确跟踪该路径曲线。

现有的路径规划算法一般是运用路径搜索算法进行搜索，而后生 成路径。但是现有的路径曲线生成方法往往没有考虑到车辆的运动学 参数约束，最终可能导致车辆无法准确进行跟踪甚至无法跟踪。而且 很多曲线生成方法中不能保证曲率连续，导致车辆跟踪时必须停下来 调整前轮转向；解决了曲率问题的方法中又大多只是解决车辆某种特 定的行为，如转向、变道等，并无一个通用的解法。

发明内容

针对现有路径曲线生成算法中对车辆运动学约束的忽视，连续曲 率问题的不满足、无法使车辆准确跟踪路径曲线以及生成方法只能解 决特定车辆行为的问题，本发明提出了一种基于回旋曲线的自动驾驶 车辆路径生成方法，采用本方法可以满足车辆运动学特性、曲率连续 约束以及统一解决车辆多种行为的路径生成问题。

为实现上述目的，本方法采用的技术方案为一种基于回旋曲线的 自动驾驶车辆路径生成方法，该方法的实现步骤如下：

步骤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)判断迭代是否停止。判定表达式如下：

满足上式，即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平行。四个基本回旋曲线组合是使用第一类 组合的两个连续对实现的。因此，在这两个组合对的交点位置确定一个共同的边界条件是很重要的。

为了解决这个问题，做出焦点切线

初始化固定在第一个回旋 曲线对

的末端，斜率为tanθ_m，其中θ_m是

的斜率角。

给出了

和

的方向约束。根据已解决的第一类问题，如果给出共同的方向约 束tanθ_m，则将两对回旋曲线分别作为两个第一类回旋问题进行解决。

迭代算法如下：

(1)参数初始化，α₁，α₂，δ₁，δ₂，ε同第一类；dα为每次迭代后 α₁的改变值；dθ为每次迭代后θ_m的改变值。tanθ_m为曲线l_m的斜率。

(2)

曲线对生成。按照步骤4中第一类情况的步骤(2) 中流程进行计算。

从P_i到P_m，

从P_f到P_m'生成。

(3)计算

端点P_m'与P_m处做出的切线l_m及切线的垂线

距离差 值D_e，

并令λ＝D_e，

(4)判断迭代是否停止。与第一类情况相同，判定表达式如下：

如果满足，即得到此时的α、δ及

并结束迭代。

若不满足上式条件，若满足λ·λ'＜0，则令

若满足 λ^⊥·λ^⊥'＜0则令

即折半迭代。之后进行：

dα＝|dα|·sign(λ)

dδ＝|dδ|·sign(λ^⊥)

α_1,2＝(α_1,2+dα)

δ_1,2＝(δ_1,2+dδ)

λ'＝λ

λ^⊥'＝λ^⊥

θ_m＝θ_m+dθ

之后返回迭代算法中的步骤(2)，直到迭代结束输出最终结果。

附图说明

图1基于回旋曲线的自动驾驶车辆路径生成方法流程图。

图2第一类迭代过程流程图(两段基本曲线)。

图3第二类迭代过程流程图(四段基本曲线)。

图4基本回旋曲线定义。

图5第一类曲线生成图。

图6规划距离差值

定义示意图。

图7第二类曲线生成图。

图8规划曲线生成仿真结果一。

图9规划曲线生成仿真结果二。

图10-1车辆换道动作路径曲线仿真(横纵坐标等比例)。

图10-2车辆换道动作路径曲线(拉开横纵坐标比例)。

具体实施方式

下面结合附图和具体实施方式对本发明作进一步说明。

本发明针对当前路径曲线生成方法中存在的无法满足车辆运动 学约束、车辆无法准确跟踪等缺陷，提出了一种基于基本回旋曲线的 自动驾驶车辆路径生成方法。该方法首先定义了基本回旋曲线对，满 足了车辆的运动学特性和轨迹曲率连续特性。之后根据终点的航向角 和始终点连线夹角的大小关系将问题分为两类分别解决。两类问题分 别采用两段和四段曲线结合的方式完成规划。所涉及方法的整体流程 图如附图1所示，附图2、附图3均为子流程图，分别为整体流程中 的第一类迭代过程和第二类迭代过程。具体实施流程分为以下步骤：

步骤1，确定起始点和终点坐标、方向、起点终点时刻的瞬时曲 率及车辆航向角，并建立坐标系：

设起点为P_i，终点为P_f。以起始点P_i时刻的车辆方向为坐标系Y 轴正方向建立坐标系，如附图4所示。θ为车辆航向角(车辆航向与 X轴夹角)，κ为曲率。δ为回旋曲线的航向角变化值，即从起点开 始航向角改变了δ。之后则起点，终点分别定义为：

P_f(x_f,y_f,θ_f,κ_f＝0)。

步骤2，定义基本回旋曲线与基本回旋曲线对：

在进入问题定义与其解决方案之前，这里需要提到关于组合方法 的一些惯例和概念。

步骤2.1，定义基本回旋曲线：

首先对曲率的符号进行定义。当车辆逆时针转动时，α(s)为正， 如C^L，车辆顺时针运动时α(s)为负，如C^R。θ(s)是通过笛卡尔坐标上 的顺时针方向的曲率积分获得的。同时δ通过从初始位置到最终位置 的方向变化来计算的。用几何连续性(方向写作G¹和曲率G²)组合C₁和C₂。其中上标R,L各自代表转向右边和左边，

表示C的逆形式， 从终点到起始点沿S→[S_l,S₀]生成。之后定义一个“基本回旋曲线”， 它的初始曲率为0并且偏向角小于90度。附图4描述了基本回旋曲 线的参数符号及其形状约定。图4中左图为曲线基本定义；中图为曲线方向性定义，通过方向性将曲线分为四种；右图为四种形状的曲率 特性图。

由回旋曲线的定义可知，回旋曲线点坐标的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_iP_f。

步骤4，根据上述两种情况分别迭代求解：

因为实际实施过程中，对α、δ进行迭代耗时较多。因此迭代开 始前，需要对参数初始值进行进一步约束。在初始步骤中，来自给定 配置的唯一信息是由d_if命名的始末端之间的配置距离(附图3中可看 到)。这意味着可能存在将α与d_if相关联的有效方式。由几何特性易 知，随着δ固定，α的增加，d_if的长度逐渐减小。这个结果反过来说， 距离d_if随着α的降低而增加。根据这个关系，一个根据距离参数决定 阿尔法的函数可以估计如下：

d_if＝Gα²

其中系数G是使用具有变量δ的二阶多项式拟合确定的。

通过实验，大体总结出参数初始化规律：

α_1i＝G·(1/d_if)²

G＝-0.3352δ²+2.2111δ-0.0429

第一类，当θ_f<φ时，如附图5所示；附图5左图为曲线结合图； 右图为曲率变化情况。迭代步骤流程如附图2所示。在这种情况下， 两个基本回旋曲线就足以构造可行路径。一个是形状为C^R的C₁，另 一个是形状为C^L的C₂.采用两支基本曲线组合满足要求，即

迭代算法如下：

(1)参数初始化。α₁为第一支基本回旋曲线C₁的曲率变化率；δ₁为 第一支基本回旋曲线C₁的航向角变化值，即C₁从P_i到P_m航向角改变了 δ₁；dα为每次迭代后α₁的改变值；dδ为每次迭代后δ₁的改变值；ε为 算法结束判定条件，即当前回旋曲线对

的终点和规划终点P_f位置的 判定参数。

(2)C₁、C₂曲线生成，并连接为一对。根据步骤2中基本曲线末 端曲率κ₁与α₁，δ₁的关系求出C₁末端曲率κ₁；并根据曲率连续特性，C₂曲率最值κ₂＝κ₁；根据曲线几何关系求出C₂曲线转向角变化量δ2：

δ₁+δ₂＝θ_i-θ_f.

上式中，θi为C₁起始点转向角，即规划起点转向角；θ_f为规划末 点转向角；接着，由δ2，κ₂求出α₂。至此，C₁、C₂曲线所有参数均以 求出，并根据步骤二定义过程进行曲线生成，并连接为基本曲线对

(3)计算

末端点P_r与规划终点P_f距离差值。在规划终点P_f沿航向 角θ_f做切线l_f；并同时在规划终点P_f做l_f的垂线

而后求出P_r与l_f距离，记作D_e；P_r与

距离，记作

定义如附图6所示。

(4)判断迭代是否停止。判定表达式如下：

满足上式，即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>φ时，如附图7所示，采用两对共四支基本曲线组 合满足要求。附图7左图为两对回旋曲线结合图；右图为曲率变化情 况。具体迭代算法步骤流程如附图3所示。两对共四支基本曲线在图 7中表示为：

每对曲线都有自己的交点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，则可以将两对回旋曲线分别做为两个第一类回旋问题进行解决。

迭代算法如下：

(1)参数初始化，α₁，α₂，δ₁，δ₂，ε同第一类问题；dα为每次迭 代后α₁的改变值；dθ为每次迭代后θ_m的改变值。tanθ_m为曲线l_m的斜 率。

(2)

曲线对生成，并连接为一对。按照第一类问题的步骤 2中流程进行计算。

从P_i到P_m，

反向，从P_f到P_m'生成。

(3)计算

端点P_m'与P_m处做出的切线l_m及切线的垂线

距离差 值D_e，

并令λ＝D_e，

(4)判断迭代是否停止。与第一类问题相同，判定表达式如下：

如果满足，即得到此时的α、δ及

并结束迭代。

若不满足上式条件，若满足λ·λ'＜0，则令

若满足 λ^⊥·λ^⊥'＜0则令

即折半迭代。之后进行：

dα＝|dα|·sign(λ)

dδ＝|dδ|·sign(λ^⊥)

α_1,2＝(α_1,2+dα)

δ_1,2＝(δ_1,2+dδ)

λ'＝λ

λ^⊥'＝λ^⊥

θ_m＝θ_m+dθ

之后返回迭代过程(2)，直到迭代结束输出最终结果。

图8，图9分别为曲线规划生成的两种结果。当使用

P_f(6,8,θ_f,0)作为始终点；车辆轴距2m；θ_f从-10°到30°以每次10°变化。 此时θ_f<φ，属于第一类迭代过程，曲线如图8所示。当设定

P_f(10,12,θ_f,0)，θ_f从70°到120°以每次10°变化时，θ_f>φ，属于第二类迭 代过程，第二类过程令

交点P_m处的切线

曲线如图9所示。图10-1、图10-2中为实际路况中规划仿真，属于第二类迭代 过程，初始航向角与末端相同，曲线动作类似为换道动作。图10-1 中横纵坐标等比例，图10-2为图10-1拉开横坐标比例，更便于观察 曲线特性。既已能完成换道，转弯过程也很容易进行规划，只需将末 点航向改为向左或向右即可。综上所述，本发明所提出的基于回旋曲 线的自动驾驶车辆路径生成方法具有较好的规划效果和较高的算法 可行性。规划出的曲线满足运动学约束以及曲率连续特性。

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

Claims (1)

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_iP_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做切线

并同时在规划终点P_f做

的垂线

而后求出P_r与

距离，记作D_e；P_r与

距离，记作

(4)判断迭代是否停止；判定表达式如下：

满足上式，即P_f与P_r垂直终点航向方向的偏差小于阈值ε，同时沿航向的偏差大于或等于0；|D_e|<ε使

末端点P_r基本在

上，而满足此条件的话，即便

也只是再沿

方向直线规划一段

即可到达P_f；如果满足，即得到此时的α、ε及

并结束迭代；

若不满足上式条件，则进行以下参数调整计算：

λ_pr(pt)＝-tan(θ_r)·xt+tan(θ_r)·x_r+y_t-y_r

其中，λ_pr(pt)为检查目标点P_t和参照点P_r的几何关系并决定是否对参数进行二分调整的重要参数；同时，由几何关系可知：

若满足λ·λ'＜0，则令

若满足

则令

即折半迭代；再使：

dα＝|dα|·sign(λ)

α₁＝(α₁+dα)

δ₁＝(δ₁+dδ)

λ'＝λ

之后返回迭代过程(2)，直到迭代结束输出最终结果；

第二类，当θ_f>φ时，采用两对共四支基本曲线组合满足要求；

每对曲线都有自己的交点P_m1，P_m2；由几何特性，P_m1，P_m2也都是曲率最大的点，因此P_m1，P_m2的切线

平行，且与P_iP_f的连线

平行；四个基本回旋曲线组合是使用第一类组合的两个连续对实现的；因此，在这两个组合对的交点位置需要确定一个共同的边界条件；

做出焦点切线

初始化固定在第一个回旋曲线对

的末端，斜率为tanθ_m，其中θ_m是

的斜率角；

给出了

和

的方向约束；根据已解决的第一类问题，如果给出共同的方向约束tanθ_m，则将两对回旋曲线分别做为两个第一类回旋问题进行解决；

迭代算法如下：

(1)参数初始化，α₁，α₂，δ₁，δ₂，ε同第一类；dα为每次迭代后α₁的改变值；dθ为每次迭代后θ_m的改变值；tanθ_m为曲线

的斜率；

(2)

曲线对生成；按照步骤4中第一类情况的步骤(2)中流程进行计算；

从P_i到P_m，

从P_f到P_m'生成；

(3)计算

端点P_m'与P_m处做出的切线

及切线的垂线

距离差值D_e，

并令λ＝D_e，

(4)判断迭代是否停止；与第一类情况相同，判定表达式如下：

如果满足，即得到此时的α、δ及

并结束迭代；

若不满足上式条件，若满足λ·λ'＜0，则令

若满足λ^⊥·λ^⊥'＜0则令

即折半迭代；之后进行：

dα＝|dα|·sign(λ)

α_1,2＝(α_1,2+dα)

δ_1,2＝(δ_1,2+dδ)

λ'＝λ

θ_m＝θ_m+dθ

之后返回迭代算法中的步骤(2)，直到迭代结束输出最终结果。

Images