《基于多项式的UUV空间轨迹规划方法.pdf》由会员分享,可在线阅读,更多相关《基于多项式的UUV空间轨迹规划方法.pdf(16页完整版)》请在专利查询网上搜索。
1、(10)申请公布号 CN 104020770 A (43)申请公布日 2014.09.03 CN 104020770 A (21)申请号 201410264172.X (22)申请日 2014.06.13 G05D 1/02(2006.01) (71)申请人 哈尔滨工程大学 地址 150001 黑龙江省哈尔滨市南岗区南通 大街 145 号哈尔滨工程大学科技处知 识产权办公室 (72)发明人 张伟 陈海田 徐达 张明臣 严浙平 (54) 发明名称 基于多项式的 UUV 空间轨迹规划方法 (57) 摘要 本发明公开了一种能够规划出三维空间曲线 的基于多项式的 UUV 空间轨迹规划方法。包括以 下几。
2、个步骤 : UUV 利用传感器获得当前自身位姿、 速度信息作为轨迹规划的起点信息, 接收轨迹规 划的终点信息, 接收障碍物信息 ; 利用多项式参 数化空间轨迹方程 ; 求解空间轨迹方程的固定系 数 ; 求解空间轨迹方程的可调系数的最优值 ; 判 断空间轨迹是否满足无碰条件, 如果满足无碰条 件, 将可调系数的最优值代入空间轨迹方程, 并输 出空间轨迹, 如果不满足无碰条件, 求解可调系数 的次优值, 并将可调系数的次优值代入空间轨迹 方程, 并输出空间轨迹。 本发明通过可调系数的选 取来获得一条空间无碰的、 最优的空间曲线轨迹。 (51)Int.Cl. 权利要求书 3 页 说明书 8 页 附图。
3、 4 页 (19)中华人民共和国国家知识产权局 (12)发明专利申请 权利要求书3页 说明书8页 附图4页 (10)申请公布号 CN 104020770 A CN 104020770 A 1/3 页 2 1. 基于多项式的 UUV 空间轨迹规划方法, 其特征在于, 包括以下几个步骤 : 步骤一 : UUV 利用传感器获得当前自身位姿、 速度信息作为轨迹规划的起点信 息, 起点信息包括初始时间 t0、 初始速度 v0、 初始位姿 q0 (x0,y0,z0,0,0,0), UUV 接收轨迹规划的终点信息, 终点信息包括终点时间 tf、 终点速度 vf、 终点位姿 qf (xf,yf,zf,f,f,。
4、f), UUV 接收障碍物信息 ; 步骤二 : 利用多项式参数化空间轨迹方程, 得到空间轨迹方程的固定系数与可调系 数 ; 步骤三 : 利用轨迹规划的起点信息和终点信息求解空间轨迹方程的固定系数 ; 步骤四 : 建立轨迹优化的性能指标, 求解空间轨迹方程的可调系数的最优值 ; 步骤五 : 判断空间轨迹是否满足无碰条件, 如果满足无碰条件, 将可调系数的最优值代 入空间轨迹方程, 并输出空间轨迹, 如果不满足无碰条件, 求解可调系数的次优值, 并将可 调系数的次优值代入空间轨迹方程, 并输出空间轨迹。 2. 根据权利要求 1 所述的基于多项式的 UUV 空间轨迹规划方法, 其特征在于 : 所述的。
5、 利用多项式参数化的空间轨迹方程为 : 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 a3T (B1)-1Y1 b0 b1 b2 b3T (B。
6、2)-1(Y2-A2b4) c0 c1 c2 c3T (B3)-1(Y3-A3c4) 其中各向量为 : B3 B1 权 利 要 求 书 CN 104020770 A 2 2/3 页 3 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)-1A。
7、3 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, 且 : 权 利 要 求 书 CN 104020770 A 3 3/3 页 4 6. 根据权利要求 4 或 5 所述的基于多项式的 UUV 空间轨迹规划方法, 其特征在于 : 所 述的空间轨迹的无碰条件为 : 其中, (xp,yp,。
8、zp) 表示山形障碍的山峰的位置坐标, 可调系数的次优值在以最优值为中心的椭圆上, 椭圆方程为 : 其中, i 1,2,3., 0 是步长, 通过不断叠加 i, 即 i i+1, 求得满足空间轨迹 的无碰条件的可调系数的次优值 : 。 权 利 要 求 书 CN 104020770 A 4 1/8 页 5 基于多项式的 UUV 空间轨迹规划方法 技术领域 0001 本发明属于一种轨迹规划方法, 尤其涉及一种基于多项式的 UUV 空间轨迹规划方 法。 背景技术 0002 随着科学技术的发展, 海洋资源开发的变化, 海洋利用区域不断扩大, 并不断向深 海、 远海延伸, 认识海洋、 开发海洋需要各种高。
9、技术手段。作为探索海洋空间的最重要手段 之一的无人水下航行器 (UUV) 技术与探索外空间的运载火箭技术有同等重要的意义, 它具 有活动范围大、 潜水深、 机动性好、 安全、 智能化、 运行和维护费用低等优点。但 UUV 自身携 带能源有限, 工作时间和距离都受到了限制, 需要进行水下能源补给和数据交换, 此时就必 须要求 UUV 能够进行回收。 0003 运 动 母 船 水 下 自 主 回 收 UUV 的 过 程 分 为 三 个 典 型 的 阶 段 : 集 合 阶 段 (Rendezvous)、 回坞阶段 (Homing) 和对接阶段 (Docking)。集合阶段的任务是空间轨迹规 划和空间。
10、轨迹跟踪。空间轨迹规划是 UUV 自主回收中的第一步, 且轨迹规划的好坏直接影 响回收过程的安全性和准确性。 0004 目前, 国内对无人水下航行器轨迹规划问题做了大量的研究。冉红阁提出了一种 基于 FMM 法的路径规划算法, FMM 法属于栅格模型规划方法, 最后得到了不同地图模型下 AUV 的轨迹 ( 冉红阁 . 自主式水下机器人的路径规划与路径跟踪方法研究 M. 中国海洋 大学 ,2009)。曹江丽博士提出了一种基于 Q 学习的避碰路径规划方法, 采用 Q 学习对机器 人反应式行为进行自主学习, 以产生优化的行为, 这种方法充分提高了系统运行速度 ( 曹 江丽 . 水下机器人路径规划问题。
11、的关键技术研究 D. 哈尔滨工程大学 ,2009)。吴小平、 冯正平等人采用蚁群算法结合 TSP 问题解决了多 AUV 轨迹规划问题, 并仿真验证了该方法 具有耗时短、 效率高的特点 ( 吴小平 , 冯正平等 . 基于蚁群算法的多 AUV 路径规划仿真研 究 J. 计算机仿真 ,2009,26(1):150-153)。李欣、 朱大奇在传统人工势场法的基础上提 出了一种改进势场函数的 AUV 轨迹规划方法, 能够实现 AUV 对静态、 动态障碍物进行很好 避碰 ( 李欣 , 朱大奇 . 基于人工势场法的自制水下机器人路径规划 J. 上海海事大学学 报 .2010,31(2):35-39)。 发明。
12、内容 0005 本发明的目的是提供一种能够规划出三维空间曲线的基于多项式的 UUV 空间轨 迹规划方法。 0006 本发明是通过以下技术方案实现的 : 0007 基于多项式的 UUV 空间轨迹规划方法, 包括以下几个步骤 : 0008 步骤一 : UUV 利用传感器获得当前自身位姿、 速度信息作为轨迹规划的起点 信息, 起点信息包括初始时间 t0、 初始速度 v0、 初始位姿 q0 (x0,y0,z0,0,0,0), UUV 接收轨迹规划的终点信息, 终点信息包括终点时间 tf、 终点速度 vf、 终点位姿 qf 说 明 书 CN 104020770 A 5 2/8 页 6 (xf,yf,zf。
13、,f,f,f), UUV 接收障碍物信息 ; 0009 步骤二 : 利用多项式参数化空间轨迹方程, 得到空间轨迹方程的固定系数与可调 系数 ; 0010 步骤三 : 利用轨迹规划的起点信息和终点信息求解空间轨迹方程的固定系数 ; 0011 步骤四 : 建立轨迹优化的性能指标, 求解空间轨迹方程的可调系数的最优值 ; 0012 步骤五 : 判断空间轨迹是否满足无碰条件, 如果满足无碰条件, 将可调系数的最优 值代入空间轨迹方程, 并输出空间轨迹, 如果不满足无碰条件, 求解可调系数的次优值, 并 将可调系数的次优值代入空间轨迹方程, 并输出空间轨迹。 0013 本发明基于多项式的 UUV 空间轨。
14、迹规划方法还可以包括 : 0014 1、 利用多项式参数化的空间轨迹方程为 : 0015 x(t) a0+a1t+a2t2+a3t3 0016 y(x) b0+b1x+b2x2+b3x3+b4x4 0017 z(t) c0+c1t+c2t2+c3t3+c4t4 0018 其中, x(t)、 y(x)、 z(t) 为空间轨迹的状态量, a0,a1,a2,a3; b0,b1,b2,b3; c0,c1,c2,c3 为空间轨迹方程的固定系数, b4, c4为空间轨迹方程的可调系数。 0019 2、 空间轨迹方程的固定系数为 : 0020 a0 a1 a2 a3T (B1)-1Y1 0021 b0 b1。
15、 b2 b3T (B2)-1(Y2-A2b4) 0022 c0 c1 c2 c3T (B3)-1(Y3-A3c4) 0023 其中各向量为 : 0024 0025 B3 B1 0026 0027 0028 3、 轨迹优化的性能指标为 : 说 明 书 CN 104020770 A 6 3/8 页 7 0029 0030 其中, (x0,y0,z0) 是起点位置的坐标, (xf,yf,zf) 是终点位置的坐标, 0031 满足轨迹优化的性能指标取最小值的可调系数的最优值为 : 0032 0033 其中 : 0034 0035 0036 0037 0038 0039 f1 x4-1 x x2 x3(。
16、B2)-1A2 0040 f2 1 x x2 x3(B2)-1Y2 0041 f3 t4-1 t t2 t3(B3)-1A3。 0042 f4 1 t t2 t3(B3)-1Y3 0043 4、 轨迹优化的性能指标简化为 : 0044 0045 (xl,yl,zl) 是连接起点位置和终点位置的初始直线上的点, 初始直线在三维空间 的表达式为 : 0046 yl Ky(xl-xk)+yk 0047 zl Kz(xl-xk)+zk 0048 式中, xk xl xf, 且 : 0049 0050 5、 空间轨迹的无碰条件为 : 0051 0052 其中, (xp,yp,zp) 表示山形障碍的山峰的。
17、位置坐标, 0053 可调系数的次优值在以最优值为中心的椭圆上, 椭圆方程为 : 说 明 书 CN 104020770 A 7 4/8 页 8 0054 0055 其中, i 1,2,3., 0 是步长, 通过不断叠加 i, 即 i i+1, 求得满足空间 轨迹的无碰条件的可调系数的次优值 : 0056 0057 0058 本发明的有益效果 : 0059 一般的路径规划方法都是实现平面的、 折线形式的轨迹, 而本发明采用多项式参 数化空间轨迹, 通过可调系数的选取来获得一条空间无碰的、 最优的空间曲线 ; 并且 UUV 在 按照轨迹航行时, 当UUV探测到新的障碍物有可以再次调用本方法, 使U。
18、UV具备探测到碍航 物后的在线重规划能力。 附图说明 0060 图 1 为本发明的流程图 ; 0061 图 2 为 UUV 的三维空间轨迹规划图 ; 0062 图 3 为简化后的性能指标求解示意 ; 0063 图 4 为可调系数求解示意图 ; 0064 图 5 为本发明的单个碍航物空间轨迹规划仿真 ; 0065 图 6 为单个碍航物空间轨迹规划仿真参数表。 具体实施方式 0066 下面结合附图对本发明做进一步详细说明。 0067 如图 1 所示, 本发明包括以下几个步骤 : 0068 步骤一 : 利用传感器获得 UUV 当前自身位姿、 速度信息作为轨迹规划的起点 信息, 起点信息包括初始时间 。
19、t0、 初始速度 v0、 初始位姿 q0 (x0,y0,z0,0,0,0), 接收轨迹规划的终点信息, 终点信息包括终点时间 tf、 终点速度 vf、 终点位姿 qf (xf,yf,zf,f,f,f), 接收障碍物信息 ; 0069 在海洋环境中, 如图 2 所示, 碍航物通常是突出于海底的山形障碍。对于这种碍航 物的建模, 采用椭圆抛物面模型。在惯性坐标系下, 用椭圆抛物面来建模山形障碍时, 其表 达式为 : 0070 0071 (x,y,z) 表示空间任意一点的坐标, (xp,yp,zp) 表示山形障碍的山峰的位置坐标, 参数 m 和 n 是用来改变碍航物山形障碍形状大小的调整变量。 00。
20、72 步骤二 : 利用多项式参数化空间轨迹方程 ; 0073 利用多项式参数化空间轨迹方程为 : 说 明 书 CN 104020770 A 8 5/8 页 9 0074 x(t) a0+a1t+a2t2+a3t3 0075 y(x) b0+b1x+b2x2+b3x3+b4x4 (2) 0076 z(t) c0+c1t+c2t2+c3t3+c4t4 0077 其中, x(t)、 y(x)、 z(t) 为空间轨迹的状态量, a0,a1,a2,a3; b0,b1,b2,b3; c0,c1,c2,c3 为空间轨迹方程的固定系数, b4, c4为空间轨迹方程的可调系数 ; 轨迹的 x 和 z 状态量采用。
21、 的参数为时间 t, 轨迹的 y 状态量采用的参数为 x, y 实际上也是时间 t, 因为 x 是时间 t 的 函数。从上面描述可知, 各多项式的最小阶数为 3 阶, 但是考虑轨迹避开碍航物所需要轨迹 需具有一定的灵活性, 所以设计的轨迹方程中将y状态量和z状态量的阶数提高到4阶。 那 么, y 状态量和 z 状态量相对应的 4 阶系数 b4,c4即为可变的轨迹优化参数。 0078 步骤三 : 求解空间轨迹方程的固定系数 ; 0079 利用边界条件即轨迹规划的起点信息和重点信息, 和空间轨迹方程, 并且将 b4,c4 作为可变量, 可以得到以下 12 个关系式 : 0080 0081 0082。
22、 0083 0084 0085 0086 0087 0088 0089 0090 0091 0092 其 中, x(t0),x(tf),y(t0),y(tf),z(t0), z(tf),均为由起点信息和终点信息得到的边界条件, 由此得到空间轨迹方程的 固定系数 : 0093 a0 a1 a2 a3T (B1)-1Y1 0094 b0 b1 b2 b3T (B2)-1(Y2-A2b4) (3) 0095 c0 c1 c2 c3T (B3)-1(Y3-A3c4) 说 明 书 CN 104020770 A 9 6/8 页 10 0096 其中, 0097 B3 B1 0098 0099 0100 步。
23、骤四 : 求解空间轨迹方程的可调系数的最优值 ; 0101 参数化方程利用变量 (b4,c4) 参数化了一簇轨迹。不过, 一些 (b4,c4) 值将产生一 些非最优的空间轨迹。这就需要建立一个合适的性能指标来找到一个最优化的 (b4,c4) 值 以最小化轨迹长度, 因为本文采用空间轨迹的长度作为性能指标, 轨迹长度表示为 : 0102 0103 在没有任何先验信息和融入弧长数值解的前提下搜索 (b4,c4) 面需要非常大的计 算量。这里, 采用一种 “初始直线” (Initial Straight Line, ISL) 计算的方法, 初始直线是 联接起始位置和目标位置的线段。如图 3 所示, 。
24、初始直线在三维空间的表达式为 : 0104 yl Ky(xl-xk)+yk 0105 zl Kz(xl-xk)+zk 0106 式中, (xl,yl,zl) 是 ISL 初始直线上的坐标, xk xl xf, 且 : 0107 0108 性能指标可以建立为 : 0109 0110 上式所描述的性能指标的本质是用轨迹和轨迹两端连线的直线所构成的封闭区 域的面积来衡量轨迹的长度。 说 明 书 CN 104020770 A 10 7/8 页 11 0111 满足轨迹优化的性能指标取最小值的可调系数的最优值为 : 0112 0113 其中 : 0114 0115 0116 0117 0118 0119。
25、 f1 x4-1 x x2 x3(B2)-1A2 0120 f2 1 x x2 x3(B2)-1Y2 0121 f3 t4-1 t t2 t3(B3)-1A3; 0122 f4 1 t t2 t3(B3)-1Y3 0123 步骤五 : 判断空间轨迹是否满足无碰条件, 如果满足无碰条件, 将可调系数的最优 值代入空间轨迹方程, 并输出空间轨迹, 如果不满足无碰条件, 求解可调系数的次优值, 并 将可调系数的次优值代入空间轨迹方程, 并输出空间轨迹 ; 0124 定义空间轨迹任意一点 (x,y,z) 与碍航物的位置关系为 s : 0125 0126 显然, s(x,y,z) 0 表明点 (x,y,。
26、z) 在碍航物的外面, 而 s(x,y,z) 0 意味着点 (x,y,z)在碍航物的内部或者在碍航物的表面。 因此, 空间轨迹任意一点(x,y,z)与碍航物 无碰条件可以描述为 : 0127 s(x,y,z) 0 0128 参数化方程代入上式, 空间轨迹的无碰条件为 : 0129 0130 如果满足无碰条件, 将可调系数的最优值代入空间轨迹方程, 并输出空间轨迹, 如 果不满足无碰条件, 求解可调系数的次优值, 可调系数的次优值在以最优值为中心的椭圆 上, 椭圆方程为 : 0131 0132 其中, i 1,2,3., 0 是步长, 通过不断叠加 i, 即 i i+1, 直到可调系数 所在的椭。
27、圆不断向外扩展, 使得规划出的轨迹满足无碰条件, 得到满足空间轨迹的无碰条 说 明 书 CN 104020770 A 11 8/8 页 12 件的可调系数的次优值 : 0133 0134 0135 将可调系数的次优值代入空间轨迹方程, 并输出空间轨迹。 0136 如图 4 所示, 该最优解的点用 “*” 表示。如果不满足无碰条件, 由于性能指标是轮 廓是一系列以为中心的椭圆, 然后可以扩展椭圆的轮廓线直到找到第一个满足无碰 条件的点 (b4,c4), 该点对应的是具有较低性能指标的次优解。这种情况下, 最优解的 点和次优解的点在图 4 中分别用 “x” 和 “+” 表示。 0137 如图 5 。
28、所示, 仿真中, 单个碍航物的参数设置为 xp 500 ; yp 500 ; zp 36 ; m 40 ; n20 ; 图6为单个碍航物空间轨迹规划仿真参数, 其规划过程为 : UUV从初始点S移动 到终点 E, UUV 首先在 0s 时规划出一条初始最优轨迹, 初始规划的轨迹由弧和虚线表示 的弧组成。在 110s, UUV 到达 Q 点时, 称 Q 点为轨迹的分支点, 探测到障碍物, 又重新调 用本发明的方法, UUV 以分支点 Q 点为起点, 以 E 点为终点进行规划, 规划出最终实线所表 示的弧轨迹。因此本发明的轨迹规划方法可以重复调用实现 UUV 的在线冲规划。 说 明 书 CN 104020770 A 12 1/4 页 13 图 1 说 明 书 附 图 CN 104020770 A 13 2/4 页 14 图 2 图 3 说 明 书 附 图 CN 104020770 A 14 3/4 页 15 图 4 图 5 说 明 书 附 图 CN 104020770 A 15 4/4 页 16 图 6 说 明 书 附 图 CN 104020770 A 16 。