CN104020770B - 基于多项式的uuv空间轨迹规划方法 - Google Patents

Abstract

本发明公开了一种能够规划出三维空间曲线的基于多项式的UUV空间轨迹规划方法。包括以下几个步骤：UUV利用传感器获得当前自身位姿、速度信息作为轨迹规划的起点信息，接收轨迹规划的终点信息，接收障碍物信息；利用多项式参数化空间轨迹方程；求解空间轨迹方程的固定系数；求解空间轨迹方程的可调系数的最优值；判断空间轨迹是否满足无碰条件，如果满足无碰条件，将可调系数的最优值代入空间轨迹方程，并输出空间轨迹，如果不满足无碰条件，求解可调系数的次优值，并将可调系数的次优值代入空间轨迹方程，并输出空间轨迹。本发明通过可调系数的选取来获得一条空间无碰的、最优的空间曲线轨迹。

Description

基于多项式的UUV空间轨迹规划方法

技术领域

本发明属于一种轨迹规划方法，尤其涉及一种基于多项式的UUV空间轨迹规划方法。

背景技术

随着科学技术的发展，海洋资源开发的变化，海洋利用区域不断扩大，并不断向深海、远海延伸，认识海洋、开发海洋需要各种高技术手段。作为探索海洋空间的最重要手段之一的无人水下航行器(UUV)技术与探索外空间的运载火箭技术有同等重要的意义，它具有活动范围大、潜水深、机动性好、安全、智能化、运行和维护费用低等优点。但UUV自身携带能源有限，工作时间和距离都受到了限制，需要进行水下能源补给和数据交换，此时就必须要求UUV能够进行回收。

运动母船水下自主回收UUV的过程分为三个典型的阶段：集合阶段(Rendezvous)、回坞阶段(Homing)和对接阶段(Docking)。集合阶段的任务是空间轨迹规划和空间轨迹跟踪。空间轨迹规划是UUV自主回收中的第一步，且轨迹规划的好坏直接影响回收过程的安全性和准确性。

目前，国内对无人水下航行器轨迹规划问题做了大量的研究。冉红阁提出了一种基于FMM法的路径规划算法，FMM法属于栅格模型规划方法，最后得到了不同地图模型下AUV的轨迹(冉红阁.自主式水下机器人的路径规划与路径跟踪方法研究[M].中国海洋大学,2009)。曹江丽博士提出了一种基于Q学习的避碰路径规划方法，采用Q学习对机器人反应式行为进行自主学习，以产生优化的行为，这种方法充分提高了系统运行速度(曹江丽.水下机器人路径规划问题的关键技术研究[D].哈尔滨工程大学,2009)。吴小平、冯正平等人采用蚁群算法结合TSP问题解决了多AUV轨迹规划问题，并仿真验证了该方法具有耗时短、效率高的特点(吴小平,冯正平等.基于蚁群算法的多AUV路径规划仿真研究[J].计算机仿真,2009,26(1):150-153)。李欣、朱大奇在传统人工势场法的基础上提出了一种改进势场函数的AUV轨迹规划方法，能够实现AUV对静态、动态障碍物进行很好避碰(李欣,朱大奇.基于人工势场法的自制水下机器人路径规划[J].上海海事大学学报.2010,31(2):35-39)。

发明内容

本发明的目的是提供一种能够规划出三维空间曲线的基于多项式的UUV空间轨迹规划方法。

本发明是通过以下技术方案实现的：

基于多项式的UUV空间轨迹规划方法，包括以下几个步骤：

步骤一：UUV利用传感器获得当前自身位姿、速度信息作为轨迹规划的起点信息，起点信息包括初始时间t₀、初始速度v₀、初始位姿q₀＝(x₀,y₀,z₀,φ₀,θ₀,ψ₀)，UUV接收轨迹规划的终点信息，终点信息包括终点时间t_f、终点速度v_f、终点位姿q_f＝(x_f,y_f,z_f,φ_f,θ_f,ψ_f)，UUV接收障碍物信息；

步骤二：利用多项式参数化空间轨迹方程，得到空间轨迹方程的固定系数与可调系数；

步骤三：利用轨迹规划的起点信息和终点信息求解空间轨迹方程的固定系数；

步骤四：建立轨迹优化的性能指标，求解空间轨迹方程的可调系数的最优值；

步骤五：判断空间轨迹是否满足无碰条件，如果满足无碰条件，将可调系数的最优值代入空间轨迹方程，并输出空间轨迹，如果不满足无碰条件，求解可调系数的次优值，并将可调系数的次优值代入空间轨迹方程，并输出空间轨迹。

本发明基于多项式的UUV空间轨迹规划方法还可以包括：

1、利用多项式参数化的空间轨迹方程为：

x(t)＝a₀+a₁t+a₂t²+a₃t³

y(x)＝b₀+b₁x+b₂x²+b₃x³+b₄x⁴

z(t)＝c₀+c₁t+c₂t²+c₃t³+c₄t⁴

其中，x(t)、y(x)、z(t)为空间轨迹的状态量，a₀,a₁,a₂,a₃；b₀,b₁,b₂,b₃；c₀,c₁,c₂,c₃为空间轨迹方程的固定系数，b₄，c₄为空间轨迹方程的可调系数。

2、空间轨迹方程的固定系数为：

[a₀ a₁ a₂ a₃]^T＝(B₁)^-1Y₁

[b₀ b₁ b₂ b₃]^T＝(B₂)^-1(Y₂-A₂b₄)

[c₀ c₁ c₂ c₃]^T＝(B₃)^-1(Y₃-A₃c₄)

其中各向量为：

$B_1=\left[\begin{array}{cccc}1& t_0& t_0^2& t_0^3\\ 0& 1& {2t}_0& {3t}_0^2\\ 1& t_f& t_f^2& t_f^3\\ 0& 1& {2t}_f& {3t}_f^2\end{array}\right],A_2=\left[\begin{array}{c}{\left(x_0\right)}^4\\ 4{\left(x_0\right)}^3\\ {\left(x_f\right)}^4\\ 4{\left(x_f\right)}^3\end{array}\right]$

$B_2=\left[\begin{array}{cccc}1& x_0& {\left(x_0\right)}^2& {\left(x_0\right)}^3\\ 0& 1& 2x_0& 3{\left(x_0\right)}^2\\ 1& x_f& {\left(x_f\right)}^2& {\left(x_f\right)}^3\\ 0& 1& 2x_f& 3{\left(x_f\right)}^2\end{array}\right],$ B₃＝B₁

$Y_1=\left[\begin{array}{c}{x}_0\\ u_0\cos {\mathrm{\θ}}_0\cos {\mathrm{\ψ}}_0\\ x_f\\ u_f\cos {\mathrm{\θ}}_f\cos {\mathrm{\ψ}}_f\end{array}\right],Y_2=\left[\begin{array}{c}{y}_0\\ \tan {\mathrm{\ψ}}_0\\ y_f\\ \tan {\mathrm{\ψ}}_f\end{array}\right]$

$Y_3=\left[\begin{array}{c}{z}_0\\ u_0\sin {\mathrm{\θ}}_0\\ z_f\\ u_f\sin {\mathrm{\θ}}_f\end{array}\right],A_3=\left[\begin{array}{c}{\left(t_0\right)}^4\\ 4{\left(t_0\right)}^3\\ {\left(t_f\right)}^4\\ {4\left(t_f\right)}^3\end{array}\right].$

3、轨迹优化的性能指标为：

其中，(x₀,y₀,z₀)是起点位置的坐标，(x_f,y_f,z_f)是终点位置的坐标，

满足轨迹优化的性能指标取最小值的可调系数的最优值为：

$b_4^{*}=-\frac{p_2}{2p_1},c_4^{*}=-\frac{p_4}{2p_3}$

其中：

$p_1={\∫}_{x_0}^{x_f}{\left(f_1\right)}^2\mathrm{dx}$

$p_2=2{\∫}_{x_0}^{x_f}[f_1{f}_2-f_1{y}_k-f_1{K}_y(x-x_k)]\mathrm{dx}$

$p_3={\∫}_{x_0}^{x_f}{\left(f_3\right)}^2\mathrm{dx}$

$p_4=2{\∫}_{x_0}^{x_f}[f_3{f}_4-f_3{z}_k-f_3{K}_z(x-x_k)]\mathrm{dx}$

$p_5={\∫}_{x_0}^{x_f}[f_2-K_y(x-x_0)-y_0]\mathrm{dx}+{\∫}_{x_0}^{x_f}[f_4-K_z(x-x_0)-z_0]\mathrm{dx}$

f₁＝x⁴-[1 x x² x³](B₂)^-1A₂

f₂＝[1 x x² x³](B₂)^-1Y₂

f₃＝t⁴-[1 t t² t³](B₃)^-1A₃。

f₄＝[1 t t² t³](B₃)^-1Y₃

4、轨迹优化的性能指标简化为：

$J^{\′}(b_4,c_4)={\∫}_{x_0}^{x_f}[{(x-x_l)}^2+{(y-y_l)}^2+{(z-z_l)}^2]\mathrm{dx}$

(x_l,y_l,z_l)是连接起点位置和终点位置的初始直线上的点，初始直线在三维空间的表达式为：

y_l＝K_y(x_l-x_k)+y_k

z_l＝K_z(x_l-x_k)+z_k

式中，x_k≤x_l≤x_f，且：

$K_y=\frac{(y_f-y_k)}{(x_f-x_k)},K_z=\frac{(z_f-z_k)}{(x_f-x_k)}.$

5、空间轨迹的无碰条件为：

$f_3{c}_4\≤\frac{{f_1}^2}{n^2}{\left(b_4^2\right)}^2+\frac{2f_1(f_2-y_p)}{n^2}{b}_4+\frac{{(f_2-y_p)}^2}{n^2}-\frac{{(x-x_p)}^2}{m^2}+z_p-f_4,\∀t\∈\left[\begin{array}{cc}{t}_0& t_f\end{array}\right]$

其中，(x_p,y_p,z_p)表示山形障碍的山峰的位置坐标，

可调系数的次优值在以最优值为中心的椭圆上，椭圆方程为：

$\mathrm{i\δ}=p_1{(b_4+\frac{p_2}{2p_1})}^2+p_3{(c_4+\frac{p_4}{2p_3})}^2$

其中，i＝1,2,3...，δ＞0是步长，通过不断叠加i，即i＝i+1，求得满足空间轨迹的无碰条件的可调系数的次优值：

${b_4}^{\′}=b_4^{*}+\sqrt{\frac{\mathrm{i\δ}}{p_1}}\cos \mathrm{\α}$

${c_4}^{\′}=c_4^{*}+\sqrt{\frac{\mathrm{i\δ}}{p_3}}\sin \mathrm{\α}.$

本发明的有益效果：

一般的路径规划方法都是实现平面的、折线形式的轨迹，而本发明采用多项式参数化空间轨迹，通过可调系数的选取来获得一条空间无碰的、最优的空间曲线；并且UUV在按照轨迹航行时，当UUV探测到新的障碍物有可以再次调用本方法，使UUV具备探测到碍航物后的在线重规划能力。

附图说明

图1为本发明的流程图；

图2为UUV的三维空间轨迹规划图；

图3为简化后的性能指标求解示意；

图4为可调系数求解示意图；

图5为本发明的单个碍航物空间轨迹规划仿真；

图6为单个碍航物空间轨迹规划仿真参数表。

具体实施方式

下面结合附图对本发明做进一步详细说明。

如图1所示，本发明包括以下几个步骤：

步骤一：利用传感器获得UUV当前自身位姿、速度信息作为轨迹规划的起点信息，起点信息包括初始时间t₀、初始速度v₀、初始位姿q₀＝(x₀,y₀,z₀,φ₀,θ₀,ψ₀)，接收轨迹规划的终点信息，终点信息包括终点时间t_f、终点速度v_f、终点位姿q_f＝(x_f,y_f,z_f,φ_f,θ_f,ψ_f)，接收障碍物信息；

在海洋环境中，如图2所示，碍航物通常是突出于海底的山形障碍。对于这种碍航物的建模，采用椭圆抛物面模型。在惯性坐标系下，用椭圆抛物面来建模山形障碍时，其表达式为：

$z-z_p=\frac{(x-x_p)}{m^2}+\frac{(y-y_p)}{n^2}---\left(1\right)$

(x,y,z)表示空间任意一点的坐标，(x_p,y_p,z_p)表示山形障碍的山峰的位置坐标，参数m和n是用来改变碍航物山形障碍形状大小的调整变量。

步骤二：利用多项式参数化空间轨迹方程；

利用多项式参数化空间轨迹方程为：

x(t)＝a₀+a₁t+a₂t²+a₃t³

y(x)＝b₀+b₁x+b₂x²+b₃x³+b₄x⁴  (2)

z(t)＝c₀+c₁t+c₂t²+c₃t³+c₄t⁴

其中，x(t)、y(x)、z(t)为空间轨迹的状态量，a₀,a₁,a₂,a₃；b₀,b₁,b₂,b₃；c₀,c₁,c₂,c₃为空间轨迹方程的固定系数，b₄，c₄为空间轨迹方程的可调系数；轨迹的x和z状态量采用的参数为时间t，轨迹的y状态量采用的参数为x，y实际上也是时间t，因为x是时间t的函数。从上面描述可知，各多项式的最小阶数为3阶，但是考虑轨迹避开碍航物所需要轨迹需具有一定的灵活性，所以设计的轨迹方程中将y状态量和z状态量的阶数提高到4阶。那么，y状态量和z状态量相对应的4阶系数b₄,c₄即为可变的轨迹优化参数。

步骤三：求解空间轨迹方程的固定系数；

利用边界条件即轨迹规划的起点信息和重点信息，和空间轨迹方程，并且将b₄,c₄作为可变量，可以得到以下12个关系式：

$x\left(t_0\right)=a_0+a_1{t}_0+a_2{t}_0^2+a_3{t}_0^3=x_0$

$\stackrel{\·}{x}\left(t_0\right)=a_1{t}_0+2a_2{t}_0+3a_3{t}_0^2=u_0\cos {\mathrm{\θ}}_0\cos {\mathrm{\ψ}}_0$

$x\left(t_f\right)=a_0+a_1{t}_f+a_2{t}_f^2+a_3{t}_f^3=x_f$

$\stackrel{\·}{x}\left(t_f\right)=a_1{t}_f+2a_2{t}_f+3a_3{t}_f^2=u_f\cos {\mathrm{\θ}}_f\cos {\mathrm{\ψ}}_f$

$y\left(t_0\right)=y\left(x_0\right)=b_0+b_1{x}_0+b_2{x}_0^2+b_3{x}_0^3+b_4{x}_0^4{=y}_0$

$(\mathrm{dy}/\mathrm{dx}){|}_{t_0}=b_1+2b_2{x}_0+3b_3{x}_0^2+4b_4{x}_0^3=[(\mathrm{dy}/\mathrm{dt})/(\mathrm{dx}/\mathrm{dt})]{|}_{t_0}=\tan {\mathrm{\ψ}}_0$

$y\left(t_f\right)=y\left(x_f\right)=b_f+b_1{x}_f+b_2{x}_f^2+b_3{x}_f^3+b_4{x}_f^4{=y}_f$

$(\mathrm{dy}/\mathrm{dx}){|}_{t_f}=b_1+2b_2{x}_f+3b_3{x}_f^2+4b_4{x}_f^3=[(\mathrm{dy}/\mathrm{dt})/(\mathrm{dx}/\mathrm{dt})]{|}_{t_f}=\tan {\mathrm{\ψ}}_f$

$z\left(t_0\right)=c_0+c_1{t}_0+c_2{t}_0^2+c_3{t}_0^3+c_4{t}_0^4=z_0$

$\stackrel{\·}{z}\left(t_0\right)=c_1+2c_2{t}_0+3c_3{t}_0^2+4c_4{t}_0^3{=-u}_0\sin {\mathrm{\θ}}_0$

$z\left(t_f\right)=c_f+c_1{t}_f+c_2{t}_f^2+c_3{t}_f^3+c_4{t}_f^4=z_f$

$\stackrel{\·}{z}\left(t_f\right)=c_1+2c_2{t}_f+3c_3{t}_f^2+4c_4{t}_f^3{=-u}_f\sin {\mathrm{\θ}}_f$

其中，x(t₀)，x(t_f)，y(t₀)，y(t_f)，z(t₀)，z(t_f)，均为由起点信息和终点信息得到的边界条件，由此得到空间轨迹方程的固定系数：

[a₀ a₁ a₂ a₃]^T＝(B₁)^-1Y₁

[b₀ b₁ b₂ b₃]^T＝(B₂)^-1(Y₂-A₂b₄)  (3)

[c₀ c₁ c₂ c₃]^T＝(B₃)^-1(Y₃-A₃c₄)

其中， $B_1=\left[\begin{array}{cccc}1& t_0& t_0^2& t_0^3\\ 0& 1& {2t}_0& {3t}_0^2\\ 1& t_f& t_f^2& t_f^3\\ 0& 1& {2t}_f& {3t}_f^2\end{array}\right],A_2=\left[\begin{array}{c}{\left(x_0\right)}^4\\ 4{\left(x_0\right)}^3\\ {\left(x_f\right)}^4\\ 4{\left(x_f\right)}^3\end{array}\right]$

$B_2=\left[\begin{array}{cccc}1& x_0& {\left(x_0\right)}^2& {\left(x_0\right)}^3\\ 0& 1& 2x_0& 3{\left(x_0\right)}^2\\ 1& x_f& {\left(x_f\right)}^2& {\left(x_f\right)}^3\\ 0& 1& 2x_f& 3{\left(x_f\right)}^2\end{array}\right],$ B₃＝B₁

$Y_1=\left[\begin{array}{c}{x}_0\\ u_0\cos {\mathrm{\θ}}_0\cos {\mathrm{\ψ}}_0\\ x_f\\ u_f\cos {\mathrm{\θ}}_f\cos {\mathrm{\ψ}}_f\end{array}\right],Y_2=\left[\begin{array}{c}{y}_0\\ \tan {\mathrm{\ψ}}_0\\ y_f\\ \tan {\mathrm{\ψ}}_f\end{array}\right]$

$Y_3=\left[\begin{array}{c}{z}_0\\ u_0\sin {\mathrm{\θ}}_0\\ z_f\\ u_f\sin {\mathrm{\θ}}_f\end{array}\right],A_3=\left[\begin{array}{c}{\left(t_0\right)}^4\\ 4{\left(t_0\right)}^3\\ {\left(t_f\right)}^4\\ {4\left(t_f\right)}^3\end{array}\right].$

步骤四：求解空间轨迹方程的可调系数的最优值；

参数化方程利用变量(b₄,c₄)参数化了一簇轨迹。不过，一些(b₄,c₄)值将产生一些非最优的空间轨迹。这就需要建立一个合适的性能指标来找到一个最优化的(b₄,c₄)值以最小化轨迹长度，因为本文采用空间轨迹的长度作为性能指标，轨迹长度表示为：

在没有任何先验信息和融入弧长数值解的前提下搜索(b₄,c₄)面需要非常大的计算量。这里，采用一种“初始直线”(Initial Straight Line，ISL)计算的方法，初始直线是联接起始位置和目标位置的线段。如图3所示，初始直线在三维空间的表达式为：

y_l＝K_y(x_l-x_k)+y_k

z_l＝K_z(x_l-x_k)+z_k

式中，(x_l,y_l,z_l)是ISL初始直线上的坐标，x_k≤x_l≤x_f，且：

$K_y=\frac{(y_f-y_k)}{(x_f-x_k)},K_z=\frac{(z_f-z_k)}{(x_f-x_k)}$

性能指标可以建立为：

$J_k(b_4^k,c_4^k)={\∫}_{x_k}^{x_f}[{(x-x_l)}^2+{(y-y_l)}^2+{(z-z_l)}^2]\mathrm{dx}---\left(5\right)$

上式所描述的性能指标的本质是用轨迹和轨迹两端连线的直线所构成的封闭区域的面积来衡量轨迹的长度。

满足轨迹优化的性能指标取最小值的可调系数的最优值为：

$b_4^{*}=-\frac{p_2}{2p_1},c_4^{*}=-\frac{p_4}{2p_3}---\left(6\right)$

其中：

$p_1={\∫}_{x_0}^{x_f}{\left(f_1\right)}^2\mathrm{dx}$

$p_2=2{\∫}_{x_0}^{x_f}[f_1{f}_2-f_1{y}_k-f_1{K}_y(x-x_k)]\mathrm{dx}$

$p_3={\∫}_{x_0}^{x_f}{\left(f_3\right)}^2\mathrm{dx}$

$p_4=2{\∫}_{x_0}^{x_f}[f_3{f}_4-f_3{z}_k-f_3{K}_z(x-x_k)]\mathrm{dx}$

$p_5={\∫}_{x_0}^{x_f}[f_2-K_y(x-x_0)-y_0]\mathrm{dx}+{\∫}_{x_0}^{x_f}[f_4-K_z(x-x_0)-z_0]\mathrm{dx}$

f₁＝x⁴-[1 x x² x³](B₂)^-1A₂

f₂＝[1 x x² x³](B₂)-¹Y₂

f₃＝t⁴-[1 t t² t³](B₃)^-1A₃；

f₄＝[1 t t² t³](B₃)^-1Y₃

步骤五：判断空间轨迹是否满足无碰条件，如果满足无碰条件，将可调系数的最优值代入空间轨迹方程，并输出空间轨迹，如果不满足无碰条件，求解可调系数的次优值，并将可调系数的次优值代入空间轨迹方程，并输出空间轨迹；

定义空间轨迹任意一点(x,y,z)与碍航物的位置关系为s：

$s\left(x,y,z\right)=\frac{(x-x_p)}{m^2}+\frac{(y-y_p)}{n^2}-(z-z_p)---\left(7\right)$

显然，s(x,y,z)＞0表明点(x,y,z)在碍航物的外面，而s(x,y,z)≤0意味着点(x,y,z)在碍航物的内部或者在碍航物的表面。因此，空间轨迹任意一点(x,y,z)与碍航物无碰条件可以描述为：

s(x,y,z)＞0

参数化方程代入上式，空间轨迹的无碰条件为：

$f_3{c}_4\≤\frac{{f_1}^2}{n^2}{\left(b_4^2\right)}^2+\frac{2f_1(f_2-y_p)}{n^2}{b}_4+\frac{{(f_2-y_p)}^2}{n^2}-\frac{{(x-x_p)}^2}{m^2}+z_p-f_4,\∀t\∈\left[\begin{array}{cc}{t}_0& t_f\end{array}\right]---\left(8\right)$

如果满足无碰条件，将可调系数的最优值代入空间轨迹方程，并输出空间轨迹，如果不满足无碰条件，求解可调系数的次优值，可调系数的次优值在以最优值为中心的椭圆上，椭圆方程为：

$\mathrm{i\δ}=p_1{(b_4+\frac{p_2}{2p_1})}^2+p_3{(c_4+\frac{p_4}{2p_3})}^2$

其中，i＝1,2,3...，δ＞0是步长，通过不断叠加i，即i＝i+1，直到可调系数所在的椭圆不断向外扩展，使得规划出的轨迹满足无碰条件，得到满足空间轨迹的无碰条件的可调系数的次优值：

${b_4}^{\′}=b_4^{*}+\sqrt{\frac{\mathrm{i\δ}}{p_1}}\cos \mathrm{\α}$

${c_4}^{\′}=c_4^{*}+\sqrt{\frac{\mathrm{i\δ}}{p_3}}\sin \mathrm{\α},$

将可调系数的次优值代入空间轨迹方程，并输出空间轨迹。

如图4所示，该最优解的点用“*”表示。如果不满足无碰条件，由于性能指标是轮廓是一系列以为中心的椭圆，然后可以扩展椭圆的轮廓线直到找到第一个满足无碰条件的点(b′₄,c′₄)，该点对应的是具有较低性能指标的次优解。这种情况下，最优解的点和次优解的点在图4中分别用“x”和“+”表示。

如图5所示，仿真中，单个碍航物的参数设置为x_p＝500；y_p＝500；z_p＝36；m＝40；n＝20；图6为单个碍航物空间轨迹规划仿真参数，其规划过程为：UUV从初始点S移动到终点E，UUV首先在0s时规划出一条初始最优轨迹，初始规划的轨迹由弧和虚线表示的弧组成。在110s，UUV到达Q点时，称Q点为轨迹的分支点，探测到障碍物，又重新调用本发明的方法，UUV以分支点Q点为起点，以E点为终点进行规划，规划出最终实线所表示的弧轨迹。因此本发明的轨迹规划方法可以重复调用实现UUV的在线冲规划。

Claims (6)

1.基于多项式的无人水下航行器空间轨迹规划方法，其特征在于，包括以下几个步骤：

步骤一：无人水下航行器利用传感器获得当前自身位姿、速度信息作为轨迹规划的起点信息，起点信息包括初始时间t₀、初始速度v₀、初始位姿q₀＝(x₀,y₀,z₀,φ₀,θ₀,ψ₀)，无人水下航行器接收轨迹规划的终点信息，终点信息包括终点时间t_f、终点速度v_f、终点位姿q_f＝(x_f,y_f,z_f,φ_f,θ_f,ψ_f)，无人水下航行器接收障碍物信息；

步骤二：利用多项式参数化空间轨迹方程，得到空间轨迹方程的固定系数与可调系数；

步骤三：利用轨迹规划的起点信息和终点信息求解空间轨迹方程的固定系数；

步骤四：建立轨迹优化的性能指标，求解空间轨迹方程的可调系数的最优值；

步骤五：判断空间轨迹是否满足无碰条件，如果满足无碰条件，将可调系数的最优值代入空间轨迹方程，并输出空间轨迹，如果不满足无碰条件，求解可调系数的次优值，并将可调系数的次优值代入空间轨迹方程，并输出空间轨迹。

2.根据权利要求1所述的基于多项式的无人水下航行器空间轨迹规划方法，其特征在于：所述的利用多项式参数化的空间轨迹方程为：

x(t)＝a₀+a₁t+a₂t²+a₃t³

y(x)＝b₀+b₁x+b₂x²+b₃x³+b₄x⁴

z(t)＝c₀+c₁t+c₂t²+c₃t³+c₄t⁴

其中，x(t)、y(x)、z(t)为空间轨迹的状态量，a₀,a₁,a₂,a₃；b₀,b₁,b₂,b₃；c₀,c₁,c₂,c₃为空间轨迹方程的固定系数，b₄，c₄为空间轨迹方程的可调系数。

3.根据权利要求2所述的基于多项式的无人水下航行器空间轨迹规划方法，其特征在于：所述的空间轨迹方程的固定系数为：

[a₀ a₁ a₂ a₃]^T＝(B₁)^-1Y₁

[b₀ b₁ b₂ b₃]^T＝(B₂)^-1(Y₂-A₂b₄)

[c₀ c₁ c₂ c₃]^T＝(B₃)^-1(Y₃-A₃c₄)

其中各向量为：

$B_1=\left[\begin{array}{cccc}1& t_0& t_0^2& t_0^3\\ 0& 1& 2t_0& 3t_0^2\\ 1& t_f& t_f^2& t_f^3\\ 0& 1& 2t_f& 3t_f^2\end{array}\right],$ $A_2=\left[\begin{array}{c}{\left(x_0\right)}^4\\ 4{\left(x_0\right)}^3\\ {\left(x_f\right)}^4\\ 4{\left(x_f\right)}^3\end{array}\right]$

$B_2=\left[\begin{array}{cccc}1& x_0& {\left(x_0\right)}^2& {\left(x_0\right)}^3\\ 0& 1& 2x_0& 3{\left(x_0\right)}^2\\ 1& x_f& {\left(x_f\right)}^2& {\left(x_f\right)}^3\\ 0& 1& 2x_f& 3{\left(x_f\right)}^2\end{array}\right],$ B₃＝B₁

$Y_1=\left[\begin{array}{c}{x}_0\\ u_0\cos {\mathrm{\θ}}_0\cos {\mathrm{\ψ}}_0\\ x_f\\ u_f\cos {\mathrm{\θ}}_f\cos {\mathrm{\ψ}}_f\end{array}\right],$ $Y_2=\left[\begin{array}{c}{y}_0\\ \tan {\mathrm{\ψ}}_0\\ y_f\\ \tan {\mathrm{\ψ}}_f\end{array}\right]$

$Y_3=\left[\begin{array}{c}{z}_0\\ u_0\sin {\mathrm{\θ}}_0\\ z_f\\ u_f\sin {\mathrm{\θ}}_f\end{array}\right],$ $A_3=\left[\begin{array}{c}{\left(t_0\right)}^4\\ 4{\left(t_0\right)}^3\\ {\left(t_f\right)}^4\\ 4{\left(t_f\right)}^3\end{array}\right].$

4.根据权利要求3所述的基于多项式的无人水下航行器空间轨迹规划方法，其特征在于：所述的轨迹优化的性能指标为：

其中，(x₀,y₀,z₀)是起点位置的坐标，(x_f,y_f,z_f)是终点位置的坐标，

满足轨迹优化的性能指标取最小值的可调系数的最优值为：

$b_4^{*}=-\frac{p_2}{2p_1}$ $c_4^{*}=-\frac{p_4}{2p_3}$

其中：

$p_1={\∫}_{x_0}^{x_f}{\left(f_1\right)}^2\mathrm{dx}$

$p_2=2{\∫}_{x_0}^{x_f}[f_1{f}_2-f_1{y}_k-f_1{K}_y(x-x_k)]\mathrm{dx}$

$p_3={\∫}_{x_0}^{x_f}{\left(f_3\right)}^2\mathrm{dx}$

$p_4=2{\∫}_{x_0}^{x_f}[f_3{f}_4-f_3{z}_k-f_3{K}_z(x-x_k)]\mathrm{dx}$

$p_5={\∫}_{x_0}^{x_f}[f_2-K_y(x-x_0)-y_0]\mathrm{dx}+{\∫}_{x_0}^{x_f}[f_4-K_z(x-x_0)-z_0]\mathrm{dx}$

f₁＝x⁴-[1 x x² x³](B₂)^-1A₂

f₂＝[1 x x² x³](B₂)^-1Y₂

                                   。

f₃＝t⁴-[1 t t² t³](B₃)^-1A₃

f₄＝[1 t t² t³](B₃)^-1Y₃

5.根据权利要求4所述的基于多项式的无人水下航行器空间轨迹规划方法，其特征在于：所述的轨迹优化的性能指标简化为：

$J^{\′}(b_4,c_4)={\∫}_{x_0}^{x_f}[{(x-x_l)}^2+{(y-y_l)}^2+{(z-z_l)}^2]\mathrm{dx}$

(x_l,y_l,z_l)是连接起点位置和终点位置的初始直线上的点，初始直线在三维空间的表达式为：

y_l＝K_y(x_l-x_k)+y_k

z_l＝K_z(x_l-x_k)+z_k

式中，x_k≤x_l≤x_f，且：

$K_y=\frac{(y_f-y_k)}{(x_f-x_k)},$ $K_z=\frac{(z_f-z_k)}{(x_f-x_k)}.$

6.根据权利要求4或5所述的基于多项式的无人水下航行器空间轨迹规划方法，其特征在于：所述的空间轨迹的无碰条件为：

$f_3{c}_4\≤\frac{{f_1}^2}{n^2}{\left(b_4^2\right)}^2+\frac{2f_1(f_2-y_p)}{n^2}{b}_4+\frac{{(f_2-y_p)}^2}{n^2}-\frac{{(x-x_p)}^2}{m^2}+z_p-f_4,\∀t\∈\left[\begin{array}{cc}{t}_0& t_f\end{array}\right]$

其中，(x_p,y_p,z_p)表示山形障碍的山峰的位置坐标，

可调系数的次优值在以最优值为中心的椭圆上，椭圆方程为：

$\mathrm{i\δ}=p_1{(b_4+\frac{p_2}{2p_1})}^2+p_3{(c_4+\frac{p_4}{2p_3})}^2$

其中，i＝1,2,3…，δ＞0是步长，通过不断叠加i，即i＝i+1，求得满足空间轨迹的无碰条件的可调系数的次优值：

$\begin{array}{c}{b_4}^{\′}=b_4^{*}+\sqrt{\frac{\mathrm{i\δ}}{p_1}}\cos \mathrm{\α}\\ {c_4}^{\′}=c_4^{*}+\sqrt{\frac{\mathrm{i\δ}}{p_3}}\sin \mathrm{\α}\end{array}.$