[发明专利]基于多项式的UUV空间轨迹规划方法

权利要求书

1.基于多项式的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接收障碍物信息； 

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

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

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

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

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

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所述的基于多项式的UUV空间轨迹规划方法，其特征在于：所述的空间轨迹方程的固定系数为： 

[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₃＝B₁