书签 分享 收藏 举报 版权申诉 / 16

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

  • 上传人:e2
  • 文档编号:4521331
  • 上传时间:2018-10-18
  • 格式:PDF
  • 页数:16
  • 大小:3.12MB
  • 摘要
    申请专利号:

    CN201410264172.X

    申请日:

    2014.06.13

    公开号:

    CN104020770A

    公开日:

    2014.09.03

    当前法律状态:

    授权

    有效性:

    有权

    法律详情:

    专利权的转移IPC(主分类):G05D 1/02登记生效日:20170316变更事项:专利权人变更前权利人:边信黔变更后权利人:哈尔滨船海智能装备科技有限公司变更事项:地址变更前权利人:150001 黑龙江省哈尔滨市南岗区南通大街258号船舶大厦15楼变更后权利人:150028 黑龙江省哈尔滨高新技术产业开发区科技创新城创新创业广场1号楼中源大道15255号企业加速器1单元2层变更事项:专利权人变更前权利人:严浙平 夏国清 哈尔滨工程大学科技园发展有限公司|||专利权的转移IPC(主分类):G05D 1/02登记生效日:20170216变更事项:专利权人变更前权利人:哈尔滨工程大学变更后权利人:边信黔变更事项:地址变更前权利人:150001 黑龙江省哈尔滨市南岗区南通大街145号哈尔滨工程大学科技处知识产权办公室变更后权利人:150001 黑龙江省哈尔滨市南岗区南通大街258号船舶大厦15楼变更事项:专利权人变更后权利人:严浙平 夏国清 哈尔滨工程大学科技园发展有限公司|||授权|||实质审查的生效IPC(主分类):G05D 1/02申请日:20140613|||公开

    IPC分类号:

    G05D1/02

    主分类号:

    G05D1/02

    申请人:

    哈尔滨工程大学

    发明人:

    张伟; 陈海田; 徐达; 张明臣; 严浙平

    地址:

    150001 黑龙江省哈尔滨市南岗区南通大街145号哈尔滨工程大学科技处知识产权办公室

    优先权:

    专利代理机构:

    代理人:

    PDF完整版下载: PDF下载
    内容摘要

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

    权利要求书

    权利要求书
    1.  基于多项式的UUV空间轨迹规划方法,其特征在于,包括以下几个步骤: 
    步骤一:UUV利用传感器获得当前自身位姿、速度信息作为轨迹规划的起点信息,起点信息包括初始时间t0、初始速度v0、初始位姿q0=(x0,y0,z0,φ0,θ0,ψ0),UUV接收轨迹规划的终点信息,终点信息包括终点时间tf、终点速度vf、终点位姿qf=(xf,yf,zf,φf,θf,ψf),UUV接收障碍物信息; 
    步骤二:利用多项式参数化空间轨迹方程,得到空间轨迹方程的固定系数与可调系数; 
    步骤三:利用轨迹规划的起点信息和终点信息求解空间轨迹方程的固定系数; 
    步骤四:建立轨迹优化的性能指标,求解空间轨迹方程的可调系数的最优值; 
    步骤五:判断空间轨迹是否满足无碰条件,如果满足无碰条件,将可调系数的最优值代入空间轨迹方程,并输出空间轨迹,如果不满足无碰条件,求解可调系数的次优值,并将可调系数的次优值代入空间轨迹方程,并输出空间轨迹。 

    2.  根据权利要求1所述的基于多项式的UUV空间轨迹规划方法,其特征在于:所述的利用多项式参数化的空间轨迹方程为: 
    x(t)=a0+a1t+a2t2+a3t3
    y(x)=b0+b1x+b2x2+b3x3+b4x4
    z(t)=c0+c1t+c2t2+c3t3+c4t4
    其中,x(t)、y(x)、z(t)为空间轨迹的状态量,a0,a1,a2,a3;b0,b1,b2,b3;c0,c1,c2,c3为空间轨迹方程的固定系数,b4,c4为空间轨迹方程的可调系数。 

    3.  根据权利要求2所述的基于多项式的UUV空间轨迹规划方法,其特征在于:所述的空间轨迹方程的固定系数为: 
    [a0 a1 a2 a3]T=(B1)-1Y1
    [b0 b1 b2 b3]T=(B2)-1(Y2-A2b4) 
    [c0 c1 c2 c3]T=(B3)-1(Y3-A3c4) 
    其中各向量为: 

    B3=B1



    4.  根据权利要求3所述的基于多项式的UUV空间轨迹规划方法,其特征在于:所述的轨迹优化的性能指标为: 

    其中,(x0,y0,z0)是起点位置的坐标,(xf,yf,zf)是终点位置的坐标, 
    满足轨迹优化的性能指标取最小值的可调系数的最优值为: 

    其中: 





    f1=x4-[1 x x2 x3](B2)-1A2
    f2=[1 x x2 x3](B2)-1Y2
    f3=t4-[1 t t2 t3](B3)-1A3
    f4=[1 t t2 t3](B3)-1Y3。

    5.  根据权利要求4所述的基于多项式的UUV空间轨迹规划方法,其特征在于:所述的轨迹优化的性能指标简化为: 

    (xl,yl,zl)是连接起点位置和终点位置的初始直线上的点,初始直线在三维空间的表达式 为: 
    yl=Ky(xl-xk)+yk
    zl=Kz(xl-xk)+zk
    式中,xk≤xl≤xf,且: 


    6.  根据权利要求4或5所述的基于多项式的UUV空间轨迹规划方法,其特征在于:所述的空间轨迹的无碰条件为: 

    其中,(xp,yp,zp)表示山形障碍的山峰的位置坐标, 
    可调系数的次优值在以最优值为中心的椭圆上,椭圆方程为: 

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


    说明书

    说明书基于多项式的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利用传感器获得当前自身位姿、速度信息作为轨迹规划的起点信息,起点信息包括初始时间t0、初始速度v0、初始位姿q0=(x0,y0,z0,φ0,θ0,ψ0),UUV接收轨迹规划的终点 信息,终点信息包括终点时间tf、终点速度vf、终点位姿qf=(xf,yf,zf,φf,θf,ψf),UUV接收障碍物信息;
    步骤二:利用多项式参数化空间轨迹方程,得到空间轨迹方程的固定系数与可调系数;
    步骤三:利用轨迹规划的起点信息和终点信息求解空间轨迹方程的固定系数;
    步骤四:建立轨迹优化的性能指标,求解空间轨迹方程的可调系数的最优值;
    步骤五:判断空间轨迹是否满足无碰条件,如果满足无碰条件,将可调系数的最优值代入空间轨迹方程,并输出空间轨迹,如果不满足无碰条件,求解可调系数的次优值,并将可调系数的次优值代入空间轨迹方程,并输出空间轨迹。
    本发明基于多项式的UUV空间轨迹规划方法还可以包括:
    1、利用多项式参数化的空间轨迹方程为:
    x(t)=a0+a1t+a2t2+a3t3
    y(x)=b0+b1x+b2x2+b3x3+b4x4
    z(t)=c0+c1t+c2t2+c3t3+c4t4
    其中,x(t)、y(x)、z(t)为空间轨迹的状态量,a0,a1,a2,a3;b0,b1,b2,b3;c0,c1,c2,c3为空间轨迹方程的固定系数,b4,c4为空间轨迹方程的可调系数。
    2、空间轨迹方程的固定系数为:
    [a0 a1 a2 a3]T=(B1)-1Y1
    [b0 b1 b2 b3]T=(B2)-1(Y2-A2b4)
    [c0 c1 c2 c3]T=(B3)-1(Y3-A3c4)
    其中各向量为:
    B1=1t0t02t03012t03t021tftf2tf3012tf3tf2,A2=(x0)44(x0)3(xf)44(xf)3]]>
    B2=1x0(x0)2(x0)3012x03(x0)21xf(xf)2(xf)3012xf3(xf)2,]]>B3=B1
    Y1=x0u0cosθ0cosψ0xfufcosθfcosψf,Y2=y0tanψ0yftanψf]]>
    Y3=z0u0sinθ0zfufsinθf,A3=(t0)44(t0)3(tf)44(tf)3.]]>
    3、轨迹优化的性能指标为:

    其中,(x0,y0,z0)是起点位置的坐标,(xf,yf,zf)是终点位置的坐标,
    满足轨迹优化的性能指标取最小值的可调系数的最优值为:
    b4*=-p22p1,c4*=-p42p3]]>
    其中:
    p1=∫x0xf(f1)2dx]]>
    p2=2∫x0xf[f1f2-f1yk-f1Ky(x-xk)]dx]]>
    p3=∫x0xf(f3)2dx]]>
    p4=2∫x0xf[f3f4-f3zk-f3Kz(x-xk)]dx]]>
    p5=∫x0xf[f2-Ky(x-x0)-y0]dx+∫x0xf[f4-Kz(x-x0)-z0]dx]]>
    f1=x4-[1 x x2 x3](B2)-1A2
    f2=[1 x x2 x3](B2)-1Y2
    f3=t4-[1 t t2 t3](B3)-1A3。
    f4=[1 t t2 t3](B3)-1Y3
    4、轨迹优化的性能指标简化为:
    J(b4,c4)=∫x0xf[(x-xl)2+(y-yl)2+(z-zl)2]dx]]>
    (xl,yl,zl)是连接起点位置和终点位置的初始直线上的点,初始直线在三维空间的表达式为:
    yl=Ky(xl-xk)+yk
    zl=Kz(xl-xk)+zk
    式中,xk≤xl≤xf,且:
    Ky=(yf-yk)(xf-xk),Kz=(zf-zk)(xf-xk).]]>
    5、空间轨迹的无碰条件为:
    f3c4f12n2(b42)2+2f1(f2-yp)n2b4+(f2-yp)2n2-(x-xp)2m2+zp-f4,∀t∈t0tf]]>
    其中,(xp,yp,zp)表示山形障碍的山峰的位置坐标,
    可调系数的次优值在以最优值为中心的椭圆上,椭圆方程为:
    =p1(b4+p22p1)2+p3(c4+p42p3)2]]>
    其中,i=1,2,3...,δ>0是步长,通过不断叠加i,即i=i+1,求得满足空间轨迹的无碰条件的可调系数的次优值:
    b4=b4*+p1cosα]]>
    c4=c4*+p3sinα.]]>
    本发明的有益效果:
    一般的路径规划方法都是实现平面的、折线形式的轨迹,而本发明采用多项式参数化空间轨迹,通过可调系数的选取来获得一条空间无碰的、最优的空间曲线;并且UUV在按照轨迹航行时,当UUV探测到新的障碍物有可以再次调用本方法,使UUV具备探测到碍航物后的在线重规划能力。
    附图说明
    图1为本发明的流程图;
    图2为UUV的三维空间轨迹规划图;
    图3为简化后的性能指标求解示意;
    图4为可调系数求解示意图;
    图5为本发明的单个碍航物空间轨迹规划仿真;
    图6为单个碍航物空间轨迹规划仿真参数表。
    具体实施方式
    下面结合附图对本发明做进一步详细说明。
    如图1所示,本发明包括以下几个步骤:
    步骤一:利用传感器获得UUV当前自身位姿、速度信息作为轨迹规划的起点信息,起点 信息包括初始时间t0、初始速度v0、初始位姿q0=(x0,y0,z0,φ0,θ0,ψ0),接收轨迹规划的终点信息,终点信息包括终点时间tf、终点速度vf、终点位姿qf=(xf,yf,zf,φf,θf,ψf),接收障碍物信息;
    在海洋环境中,如图2所示,碍航物通常是突出于海底的山形障碍。对于这种碍航物的建模,采用椭圆抛物面模型。在惯性坐标系下,用椭圆抛物面来建模山形障碍时,其表达式为:
    z-zp=(x-xp)m2+(y-yp)n2---(1)]]>
    (x,y,z)表示空间任意一点的坐标,(xp,yp,zp)表示山形障碍的山峰的位置坐标,参数m和n是用来改变碍航物山形障碍形状大小的调整变量。
    步骤二:利用多项式参数化空间轨迹方程;
    利用多项式参数化空间轨迹方程为:
    x(t)=a0+a1t+a2t2+a3t3
    y(x)=b0+b1x+b2x2+b3x3+b4x4  (2)
    z(t)=c0+c1t+c2t2+c3t3+c4t4
    其中,x(t)、y(x)、z(t)为空间轨迹的状态量,a0,a1,a2,a3;b0,b1,b2,b3;c0,c1,c2,c3为空间轨迹方程的固定系数,b4,c4为空间轨迹方程的可调系数;轨迹的x和z状态量采用的参数为时间t,轨迹的y状态量采用的参数为x,y实际上也是时间t,因为x是时间t的函数。从上面描述可知,各多项式的最小阶数为3阶,但是考虑轨迹避开碍航物所需要轨迹需具有一定的灵活性,所以设计的轨迹方程中将y状态量和z状态量的阶数提高到4阶。那么,y状态量和z状态量相对应的4阶系数b4,c4即为可变的轨迹优化参数。
    步骤三:求解空间轨迹方程的固定系数;
    利用边界条件即轨迹规划的起点信息和重点信息,和空间轨迹方程,并且将b4,c4作为可变量,可以得到以下12个关系式:
    x(t0)=a0+a1t0+a2t02+a3t03=x0]]>
    x·(t0)=a1t0+2a2t0+3a3t02=u0cosθ0cosψ0]]>
    x(tf)=a0+a1tf+a2tf2+a3tf3=xf]]>
    x·(tf)=a1tf+2a2tf+3a3tf2=ufcosθfcosψf]]>
    y(t0)=y(x0)=b0+b1x0+b2x02+b3x03+b4x04=y0]]>
    (dy/dx)|t0=b1+2b2x0+3b3x02+4b4x03=[(dy/dt)/(dx/dt)]|t0=tanψ0]]>
    y(tf)=y(xf)=bf+b1xf+b2xf2+b3xf3+b4xf4=yf]]>
    (dy/dx)|tf=b1+2b2xf+3b3xf2+4b4xf3=[(dy/dt)/(dx/dt)]|tf=tanψf]]>
    z(t0)=c0+c1t0+c2t02+c3t03+c4t04=z0]]>
    z·(t0)=c1+2c2t0+3c3t02+4c4t03=-u0sinθ0]]>
    z(tf)=cf+c1tf+c2tf2+c3tf3+c4tf4=zf]]>
    z·(tf)=c1+2c2tf+3c3tf2+4c4tf3=-ufsinθf]]>
    其中,x(t0),x(tf),y(t0),y(tf),z(t0),z(tf),均为由起点信息和终点信息得到的边界条件,由此得到空间轨迹方程的固定系数:
    [a0 a1 a2 a3]T=(B1)-1Y1
    [b0 b1 b2 b3]T=(B2)-1(Y2-A2b4)  (3)
    [c0 c1 c2 c3]T=(B3)-1(Y3-A3c4)
    其中,B1=1t0t02t03012t03t021tftf2tf3012tf3tf2,A2=(x0)44(x0)3(xf)44(xf)3]]>
    B2=1x0(x0)2(x0)3012x03(x0)21xf(xf)2(xf)3012xf3(xf)2,]]>B3=B1
    Y1=x0u0cosθ0cosψ0xfufcosθfcosψf,Y2=y0tanψ0yftanψf]]>
    Y3=z0u0sinθ0zfufsinθf,A3=(t0)44(t0)3(tf)44(tf)3.]]>
    步骤四:求解空间轨迹方程的可调系数的最优值;
    参数化方程利用变量(b4,c4)参数化了一簇轨迹。不过,一些(b4,c4)值将产生一些非最优 的空间轨迹。这就需要建立一个合适的性能指标来找到一个最优化的(b4,c4)值以最小化轨迹长度,因为本文采用空间轨迹的长度作为性能指标,轨迹长度表示为:

    在没有任何先验信息和融入弧长数值解的前提下搜索(b4,c4)面需要非常大的计算量。这里,采用一种“初始直线”(Initial Straight Line,ISL)计算的方法,初始直线是联接起始位置和目标位置的线段。如图3所示,初始直线在三维空间的表达式为:
    yl=Ky(xl-xk)+yk
    zl=Kz(xl-xk)+zk
    式中,(xl,yl,zl)是ISL初始直线上的坐标,xk≤xl≤xf,且:
    Ky=(yf-yk)(xf-xk),Kz=(zf-zk)(xf-xk)]]>
    性能指标可以建立为:
    Jk(b4k,c4k)=∫xkxf[(x-xl)2+(y-yl)2+(z-zl)2]dx---(5)]]>
    上式所描述的性能指标的本质是用轨迹和轨迹两端连线的直线所构成的封闭区域的面积来衡量轨迹的长度。
    满足轨迹优化的性能指标取最小值的可调系数的最优值为:
    b4*=-p22p1,c4*=-p42p3---(6)]]>
    其中:
    p1=∫x0xf(f1)2dx]]>
    p2=2∫x0xf[f1f2-f1yk-f1Ky(x-xk)]dx]]>
    p3=∫x0xf(f3)2dx]]>
    p4=2∫x0xf[f3f4-f3zk-f3Kz(x-xk)]dx]]>
    p5=∫x0xf[f2-Ky(x-x0)-y0]dx+∫x0xf[f4-Kz(x-x0)-z0]dx]]>
    f1=x4-[1 x x2 x3](B2)-1A2
    f2=[1 x x2 x3](B2)-1Y2
    f3=t4-[1 t t2 t3](B3)-1A3;
    f4=[1 t t2 t3](B3)-1Y3
    步骤五:判断空间轨迹是否满足无碰条件,如果满足无碰条件,将可调系数的最优值代入空间轨迹方程,并输出空间轨迹,如果不满足无碰条件,求解可调系数的次优值,并将可调系数的次优值代入空间轨迹方程,并输出空间轨迹;
    定义空间轨迹任意一点(x,y,z)与碍航物的位置关系为s:
    s(x,y,z)=(x-xp)m2+(y-yp)n2-(z-zp)---(7)]]>
    显然,s(x,y,z)>0表明点(x,y,z)在碍航物的外面,而s(x,y,z)≤0意味着点(x,y,z)在碍航物的内部或者在碍航物的表面。因此,空间轨迹任意一点(x,y,z)与碍航物无碰条件可以描述为:
    s(x,y,z)>0
    参数化方程代入上式,空间轨迹的无碰条件为:
    f3c4f12n2(b42)2+2f1(f2-yp)n2b4+(f2-yp)2n2-(x-xp)2m2+zp-f4,∀t∈t0tf---(8)]]>
    如果满足无碰条件,将可调系数的最优值代入空间轨迹方程,并输出空间轨迹,如果不满足无碰条件,求解可调系数的次优值,可调系数的次优值在以最优值为中心的椭圆上,椭圆方程为:
    =p1(b4+p22p1)2+p3(c4+p42p3)2]]>
    其中,i=1,2,3...,δ>0是步长,通过不断叠加i,即i=i+1,直到可调系数所在的椭圆不断向外扩展,使得规划出的轨迹满足无碰条件,得到满足空间轨迹的无碰条件的可调系数的次优值:
    b4=b4*+p1cosα]]>
    c4=c4*+p3sinα,]]>
    将可调系数的次优值代入空间轨迹方程,并输出空间轨迹。
    如图4所示,该最优解的点用“*”表示。如果不满足无碰条件,由于性能指标是轮廓是一系列以为中心的椭圆,然后可以扩展椭圆的轮廓线直到找到第一个满足无碰条件的点(b′4,c′4),该点对应的是具有较低性能指标的次优解。这种情况下,最优解的点和次优解的点在图4中分别用“x”和“+”表示。
    如图5所示,仿真中,单个碍航物的参数设置为xp=500;yp=500;zp=36;m=40;n=20;图6为单个碍航物空间轨迹规划仿真参数,其规划过程为:UUV从初始点S移动到终点E,UUV首先在0s时规划出一条初始最优轨迹,初始规划的轨迹由弧和虚线表示的弧组成。在110s,UUV到达Q点时,称Q点为轨迹的分支点,探测到障碍物,又重新调用本发明的方法,UUV以分支点Q点为起点,以E点为终点进行规划,规划出最终实线所表示的弧轨迹。因此本发明的轨迹规划方法可以重复调用实现UUV的在线冲规划。

    关 键  词:
    基于 多项式 UUV 空间 轨迹 规划 方法
      专利查询网所有文档均是用户自行上传分享,仅供网友学习交流,未经上传用户书面授权,请勿作他用。
    0条评论

    还可以输入200字符

    暂无评论,赶快抢占沙发吧。

    关于本文
    本文标题:基于多项式的UUV空间轨迹规划方法.pdf
    链接地址:https://www.zhuanlichaxun.net/p-4521331.html
    关于我们 - 网站声明 - 网站地图 - 资源地图 - 友情链接 - 网站客服 - 联系我们

    copyright@ 2017-2018 zhuanlichaxun.net网站版权所有
    经营许可证编号:粤ICP备2021068784号-1