[发明专利]基于多项式的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⁴