一种基于Dubins曲线的欠驱动AUV路径规划方法与流程

一种基于dubins曲线的欠驱动auv路径规划方法
技术领域
1.本发明涉及自主水下运载器，具体涉及一种基于dubins曲线的欠驱动auv路径规划方法。


背景技术：

2.目前，因为自主水下运载器（auv）具有不需要载人、无遥控电缆、机动性好、活动范围大等优点，所以在海洋开发中应用越来越广泛。auv能够完成水下打捞与情报收集、海洋科学研究、水下管道与结构物检查、水下武器对抗与反潜等任务。随着auv的发展及数量的增多，auv家族里面出现了一类特殊的成员，称为欠驱动auv，欠驱动auv用较少的控制输入实现其运动的控制。auv设计为欠驱动的形式可以减少控制设备、提高其机械的可靠性、降低其制造成本和减轻重量。auv在水下执行任务之前必须对其路径进行规划。目前常用的路径规划方法是先确定auv经过的关键路径点，然后将每两个相邻路径点用直线进行连接，如图1中的折线（绿色）路径。当只有两个路径点（起点和终点）时，则简化为auv的镇定控制问题，即控制auv从任意的初始位姿稳定到最终的期望位姿。auv的镇定控制常用于其对接或回收时的过程控制。虽然用直线连接路径点进行路径规划方法简单，但存在路径不光滑的缺点。在路径点附近由当前路径段切换为下一个路径段时，控制信号（舵角）会出现大幅的抖振现象，这不仅会增加执行机构的能耗而且不利于控制的稳定性。如果路径是光滑连续的，例如图1中的曲线（红色）路径，则路径不仅更便于被跟踪而且控制信号也会更平稳，从而更有利于节能。为了得到光滑的路径，目前常用的一种方法是在路径点处用圆弧或bessel曲线进行过渡（类似于倒圆角），但这样处理会使路径无法通过规定的路径点。另一种常用方法是采用样条曲线插值技术进行路径规划，但该方法存在计算量大的缺点。另外，通常欠驱动auv由于没有配置侧向和垂向推进器，存在最小回转半径的约束。采用样条曲线插值技术规划的路径往往由于局部曲率过大而无法被欠驱动auv有效的跟踪。


技术实现要素：

3.本发明的主要目的在于提供一种基于dubins曲线的欠驱动auv路径规划方法，通过简化dubins曲线的类型选择和参数计算，解决了欠驱动auv光滑路径的规划问题。
4.本发明采用的技术方案是：一种基于dubins曲线的欠驱动auv路径规划方法，包括dubins曲线的参数计算，具体为：pzn型dubins曲线的参数计算方法；nzp型dubins曲线的参数计算方法；nzn型dubins曲线的参数计算方法；pzp型dubins曲线的参数计算方法。
5.进一步地，所述pzn型dubins曲线的参数计算方法包括：首先将终点t看作局部坐标系的原点，按式（1）进行坐标变换：
ꢀꢀ
（1）
式中，[ξ
s
ꢀꢀ
η
s
ꢀꢀ
ψ
s
]代表起点s在惯性坐标系水平面内的位姿，[ξ
t
ꢀꢀ
η
t ꢀ
ψ
t
]代表终点t在惯性坐标系水平面内的位姿，[x
s
ꢀꢀ
y
s
ꢀꢀ
α]代表起点s在局部坐标系内的位姿，即相对于终点t的位姿；pzn型dubins曲线参数的计算方法：auv的起点为s方向为a1，姿态角为α，首先沿曲率为正、半径为r
min
、圆心为o1的圆弧旋转角度θ1，然后沿直线l航行，最后沿曲率为负、半径为r
min
、圆心为o2的圆弧旋转角度θ2到达终点t，终点t的方向为a2，姿态角为零；r
1s
、r
1f
、r
2s
、r
2f
分别为从四个切点指向圆心的向量，l代表直线路径l的长度；角度α、θ1、θ2满足等式：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（2）从起点为s到终点t的位置转换向量满足式（3）
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（3）式中，式（3）可以整理为式（4），式（4）进一步简化为式（5）
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（3）
ꢀꢀꢀꢀ
（4）
ꢀꢀ
（5）定义变量则式（5）可得定义则式（5）最终可得
ꢀꢀꢀꢀ
（6）根据公式（2）至（6），pzn型dubins曲线的参数计算公式最终整理为式（7）：
ꢀꢀꢀꢀꢀꢀꢀꢀ
（7）。
[0006]
更进一步地，所述nzp型dubins曲线的参数计算方法包括：（8）。
[0007]
更进一步地，所述nzn型dubins曲线的参数计算方法包括：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（9）。
[0008]
更进一步地，所述pzp型dubins曲线的参数计算方法包括：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（10）。
[0009]
更进一步地，所述基于dubins曲线的欠驱动auv路径规划方法，还包括：在dubins曲线参数的计算中，角度变量α、γ、θ1、θ2均为[0, 2π]的正值，如果在计算中出现负值时将其加2π处理，出现大于2π的情况则将其减2π处理。
[0010]
本发明的优点：本发明的方法考虑了欠驱动auv最小回转半径的约束，应用dubins曲线规划出一条从起点到终点由圆弧
→
直线
→
圆弧构成的最短的光滑路径。通过将起点位姿相对于终点进行坐标变换，将终点统一看作局部坐标系的原点，简化了四种类型dubins曲线的参数计算。通过将起点相对于终点的位姿在四个象限内进行分类，对dubins曲线的类型进行了全面的分析，解决了dubins曲线类型的选择判断问题。该方法有效的减小了dubins曲线参数的计算量。该方法也可以应用于到其它无人机械的路径规划，例如无人陆地车辆和无人机。
[0011]
除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照图，对本发明作进一步详细的说明。
附图说明
[0012]
构成本技术的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
[0013]
图1是自主水下运载器路径规划示意图；图2是本发明的dubins曲线的类型图；图3是本发明的dubins曲线路径规划结果图；
图4是本发明的路径跟踪仿真结果图。
具体实施方式
[0014]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0015]
当给定欠驱动auv当前路径段起点和终点的位姿后，应用dubins曲线可以规划出一条由圆弧
→
直线
→
圆弧构成的最短的光滑路径，其圆弧半径只要大于等于auv的最小回转半径，则路径满足可跟踪的约束条件。因为本发明研究的欠驱动auv不具备后退（倒车）能力，由圆弧
→
直线
→
圆弧构成的dubins曲线存在四种类型如图2所示，在本发明中分别将其命名为pzn型、nzp型、nzn型和pzp型，名称中的p代表路径为圆弧其曲率为正、n代表路径为圆弧其曲率为负、z代表路径为直线其曲率为零。当给定起点和终点的位姿后，上述四种类型的dubins曲线并不一定都是可行路径，但至少有一种类型是存在最优的路径，所以如何确定最优路径的类型和计算其参数是应用dubins曲线进行路径规划的关键问题。
[0016]
dubins曲线的参数计算：通常auv在每一段路径的起点和终点的位姿是定义在惯性坐标系中的，为了简化dubins曲线参数的计算，首先将终点t看作局部坐标系的原点，按式（1）进行坐标变换（1）式中，[ξ
s
ꢀꢀ
η
s
ꢀꢀ
ψ
s
]代表起点s在惯性坐标系水平面内的位姿，[ξ
t
ꢀꢀ
η
t
ꢀꢀ
ψ
t
]代表终点t在惯性坐标系水平面内的位姿，[x
s
ꢀꢀ
y
s
ꢀꢀ
α]代表起点s在局部坐标系内的位姿，即相对于终点t的位姿。接下来以pzn型dubins曲线为例，介绍其参数的计算方法。
[0017]
pzn型dubins曲线如图2(a)所示，auv的起点为s方向为a1，姿态角为α，首先沿曲率为正、半径为r
min
、圆心为o1的圆弧旋转角度θ1，然后沿直线l航行，最后沿曲率为负、半径为r
min
、圆心为o2的圆弧旋转角度θ2到达终点t，终点t的方向为a2，姿态角为零。图2(a)中r
1s
、r
1f
、r
2s
、r
2f
分别为从四个切点指向圆心的向量，l代表直线路径l的长度。角度α、θ1、θ2满足等式
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（2）从起点为s到终点t的位置转换向量满足式（3），
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（3）
式中，式（3）可以整理为式（4），式（4）可以进一步简化为式（5）
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（3）
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（4）
ꢀꢀꢀꢀꢀꢀꢀ
（5）定义变量则式（5）可得定义
则式（5）最终可得
ꢀꢀꢀꢀ
（6）根据公式（2）至（6），pzn型dubins曲线的参数计算公式最终可以整理为式（7）。其它三种类型的dubins曲线的参数计算方法类似，由于篇幅有限，接下来直接给出最终结果。nzp型dubins曲线的参数计算方法见公式（8），nzn型dubins曲线的参数计算方法见公式（9），pzp型dubins曲线的参数计算方法见公式（10）。在以上dubins曲线参数的计算中，角度变量α、γ、θ1、θ2均为[0, 2π]的正值，如果在计算中出现负值时将其加2π处理，出现大于2π的情况则将其减2π处理。
[0018]
（7）（8）
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（9）
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（10）。
[0019]
为了便于对dubins曲线的类型进行判断和选择，本发明将起点s相对于终点t的位置[x
s
ꢀꢀ
y
s
]和姿态α各分为四个象限，位置的第一至第四象限分别对应位置[x
s
ꢀꢀ
y
s
]的符号为[ ]、[
‑ꢀ
]、[
‑ꢀ‑
]、[
ꢀ‑
]，姿态的第一至第四象限分别对应α的取值范围为[00.5π]、[0.5π
ꢀꢀ
π]、[π
ꢀꢀ
1.5π]、[1.5π
ꢀꢀ
2π]，位置和姿态一共有十六种组合。本发明对每一种组合状态下最优的dubins曲线的类型进行了分析，结果见表1。只要根据起点s相对于终点t的位姿通过查表1即可确定最优的dubins曲线的类型，然后再通过公式（7）
‑
（10）进行计算，即可获取最优路径的参数。十六种状态中有八种存在唯一的最优解，另外八种存在两种解，只需对这两种可行路径进行计算和比较，选取路径较短的为最优路径即可。与其它文献提出的方法相比该方法计算量要小的多。
[0020]
仿真结果与分析：基于表2所列出的路径点，采用本节提出的计算方法应用dubins曲线进行路径规划获取的路径的参数见表3，图3为路径示意图，在图3中点划线为传统的基于路径点的直线路径，实线为采用dubins曲线规划出的路径。
[0021]
表1
ꢀꢀ
dubins曲线的类型选择表2 路径点表3 dubins曲线路径参数
采用美国hydroid公司的remus
‑
100型auv作为仿真模型，对这两种路径进行跟踪控制仿真和对比。根据仿真结果图4，可以看到采用直线路径进行跟踪时，在路径点附近进行路径段切换时其横向跟踪误差和艏摇角误差都出现了明显的跳变，而采用dubins曲线路径进行跟踪时误差曲线要平稳的多，因为dubins曲线路径是光滑的。通过控制信号可以看到，当采用直线路径进行跟踪时，每次路径切换转弯时推进器的转速和舵角都出现了较大的跳变，垂直舵角出现了饱和现象，而采用dubins曲线路径进行跟踪时控制信号要平稳的多，也没有出现舵角饱和现象，与直线路径跟踪相比推进器的转速、水平舵角和垂直舵角的均方差分别降低了23%、25%和55%，平方和分别降低了1.2%、42%和80%，舵角的波动现象得到了明显的改善，其转舵的总能耗降低了大约60%。
[0022]
最后，通过仿真实验验证了该方法规划出的路径可以减少auv舵角的抖振，从而有效的降低能耗。
[0023]
本发明研究了一类不具备后退（倒车）能力的欠驱动auv在镇定和路径跟踪控制中的路径规划问题。考虑了欠驱动auv最小回转半径的约束，应用dubins曲线规划出一条从起点到终点由圆弧
→
直线
→
圆弧构成的最短的光滑路径。通过将起点位姿相对于终点进行坐标变换，将终点统一看作局部坐标系的原点，简化了四种类型dubins曲线的参数计算。通过将起点相对于终点的位姿在四个象限内进行分类，对dubins曲线的类型进行了全面的分析，解决了dubins曲线类型的选择判断问题。该方法有效的减小了dubins曲线参数的计算量。该方法也可以应用于到其它无人机械的路径规划，例如无人陆地车辆和无人机。
[0024]
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。