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

摘要

本发明公开了一种能够规划出三维空间曲线的基于多项式的UUV空间轨迹规划方法。包括以下几个步骤：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接收障碍物信息； 步骤二：利用多项式参数化空间轨迹方程，得到空间轨迹方程的固定系数与可调系数； 步骤三：利用轨迹规划的起点信息和终点信息求解空间轨迹方程的固定系数； 步骤四：建立轨迹优化的性能指标，求解空间轨迹方程的可调系数的最优值； 步骤五：判断空间轨迹是否满足无碰条件，如果满足无碰条件，将可调系数的最优值代入空间轨迹方程，并输出空间轨迹，如果不满足无碰条件，求解可调系数的次优值，并将可调系数的次优值代入空间轨迹方程，并输出空间轨迹。