基于多项式的uuv空间轨迹规划方法
【专利摘要】本发明公开了一种能够规划出三维空间曲线的基于多项式的UUV空间轨迹规划方法。包括以下几个步骤：UUV利用传感器获得当前自身位姿、速度信息作为轨迹规划的起点信息，接收轨迹规划的终点信息，接收障碍物信息；利用多项式参数化空间轨迹方程；求解空间轨迹方程的固定系数；求解空间轨迹方程的可调系数的最优值；判断空间轨迹是否满足无碰条件，如果满足无碰条件，将可调系数的最优值代入空间轨迹方程，并输出空间轨迹，如果不满足无碰条件，求解可调系数的次优值，并将可调系数的次优值代入空间轨迹方程，并输出空间轨迹。本发明通过可调系数的选取来获得一条空间无碰的、最优的空间曲线轨迹。
【专利说明】基于多项式的UUV空间轨迹规划方法
【技术领域】
[0001]本发明属于一种轨迹规划方法，尤其涉及一种基于多项式的UUV空间轨迹规划方法。
【背景技术】
[0002]随着科学技术的发展，海洋资源开发的变化，海洋利用区域不断扩大，并不断向深海、远海延伸，认识海洋、开发海洋需要各种高技术手段。作为探索海洋空间的最重要手段之一的无人水下航行器(UUV)技术与探索外空间的运载火箭技术有同等重要的意义，它具有活动范围大、潜水深、机动性好、安全、智能化、运行和维护费用低等优点。但UUV自身携带能源有限，工作时间和距离都受到了限制，需要进行水下能源补给和数据交换，此时就必须要求UUV能够进行回收。
[0003]运动母船水下自主回收UUV的过程分为三个典型的阶段:集合阶段(Rendezvous)、回玛阶段(Homing)和对接阶段(Docking)。集合阶段的任务是空间轨迹规划和空间轨迹跟踪。空间轨迹规划是UUV自主回收中的第一步，且轨迹规划的好坏直接影响回收过程的安全性和准确性。
[0004]目前，国内对无人水下航行器轨迹规划问题做了大量的研究。冉红阁提出了一种基于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)。
【发明内容】
[0005]本发明的目的是提供一种能够规划出三维空间曲线的基于多项式的UUV空间轨迹规划方法。
[0006]本发明是通过以下技术方案实现的:
[0007]基于多项式的UUV空间轨迹规划方法，包括以下几个步骤:
[0008]步骤一:UUV利用传感器获得当前自身位姿、速度信息作为轨迹规划的起点信息，起点信息包括初始时间tQ、初始速度V。、初始位姿qQ = (x0, y0, ζ0, Φ0, θ 0，ψ0),UUV接收轨迹规划的终点信息，终点信息包括终点时间tf、终点速度Vf、终点位姿qf =(xf) yf, zf, φ?; Θ f，Ψ?)，UUV 接收障碍物信息；
[0009]步骤二:利用多项式参数化空间轨迹方程，得到空间轨迹方程的固定系数与可调系数；
[0010]步骤三:利用轨迹规划的起点信息和终点信息求解空间轨迹方程的固定系数；
[0011]步骤四:建立轨迹优化的性能指标，求解空间轨迹方程的可调系数的最优值；
[0012]步骤五:判断空间轨迹是否满足无碰条件，如果满足无碰条件，将可调系数的最优值代入空间轨迹方程，并输出空间轨迹，如果不满足无碰条件，求解可调系数的次优值，并将可调系数的次优值代入空间轨迹方程，并输出空间轨迹。
[0013]本发明基于多项式的UUV空间轨迹规划方法还可以包括:
[0014]1、利用多项式参数化的空间轨迹方程为:
[0015]X (t) = ao+a^+aat^agt3 [0016]y(x) = b0+b1x+b2x2+b3x3+b4x4
[0017]z (t) = c0+c1t+c2t2+c3t3+c4t4
[0018]其中，x(t)、y(X)、z (t)为空间轨迹的状态量，a0, a” a2, a3 ；b0, b” b2, b3 ；c0, C1, c2, C3为空间轨迹方程的固定系数，b4，C4为空间轨迹方程的可调系数。
[0019]2、空间轨迹方程的固定系数为:
[0020][a0 Sl1 a2 a3] = (B1) Y1
[0021 ] [b0 Id1 b2 b3]T = (B2) (Y2_A2b4)
[0022][c0 C1 C2 c3]T = (B3) (Y3-A3C4)
[0023]其中各向量为:
【权利要求】
1.基于多项式的UUV空间轨迹规划方法，其特征在于，包括以下几个步骤: 步骤一:uuv利用传感器获得当前自身位姿、速度信息作为轨迹规划的起点信息，起点信息包括初始时间td、初始速度V(!、初始位姿q(! = (x0, y0) Z0, Φ0, θ 0, ψ0),UUV接收轨迹规划的终点信息，终点信息包括终点时间tf、终点速度Vf、终点位姿qf =(xf, yf, Zf, Φr, θ f，Vf)，UUV 接收障碍物信息； 步骤二:利用多项式参数化空间轨迹方程，得到空间轨迹方程的固定系数与可调系数； 步骤三:利用轨迹规划的起点信息和终点信息求解空间轨迹方程的固定系数； 步骤四:建立轨迹优化的性能指标，求解空间轨迹方程的可调系数的最优值； 步骤五:判断空间轨迹是否满足无碰条件，如果满足无碰条件，将可调系数的最优值代入空间轨迹方程，并输出空间轨迹，如果不满足无碰条件，求解可调系数的次优值，并将可调系数的次优值代入空间轨迹方程，并输出空间轨迹。
2.根据权利要求1所述的基于多项式的UUV空间轨迹规划方法，其特征在于:所述的利用多项式参数化的空间轨迹方程为:
X (t) = a0+a1t+a2t2+a:1t:}
y (X) = b0+b1x+b2x2+b3x3+b4x4
Z (t) = c0+c1t+c2t2+c3t3+c4t4
其中，x(t)、y (X)、z (t)为空间轨迹的状态量，aQ，a” a2, a3 ；b0, b” b2, b3 ；c0, C1, c2, C3 为空间轨迹方程的固定系数，b4，C4为空间轨迹方程的可调系数。
3.根据权利要求2所述的基于多项式的UUV空间轨迹规划方法，其特征在于:所述的空间轨迹方程的固定系数为:
4.根据权利要求3所述的基于多项式的UUV空间轨迹规划方法，其特征在于:所述的轨迹优化的性能指标为:
5.根据权利要求4所述的基于多项式的UUV空间轨迹规划方法，其特征在于:所述的轨迹优化的性能指标简化为:
6.根据权利要求4或5所述的基于多项式的UUV空间轨迹规划方法，其特征在于:所述的空间轨迹的无碰条件为:
【文档编号】G05D1/02GK104020770SQ201410264172
【公开日】2014年9月3日 申请日期:2014年6月13日 优先权日:2014年6月13日
【发明者】张伟, 陈海田, 徐达, 张明臣, 严浙平 申请人:哈尔滨工程大学