Method for determining initial position of dead reckoning system of underwater vehicle
-
摘要: 为了提高航位推算(DR)系统的初始位置精度,根据水下导航传感器的特点,提出了初始定位时间内基于DR轨迹平移策略的初始位置确定方法,该方法综合了DR和水声定位系统的特点,通过平移DR轨迹,得到了在确定运动趋势约束下的最优轨迹,并依据轨迹求出初始位置.针对某次海试数据,其结果表明:应用本文方法确定的初始位置误差分别为应用平均值法和最小二乘拟合方法的60.2%和77.2%,应用此方法可减小DR误差,并能提高DR/水声组合定位系统的稳定性.Abstract: According to the characteristics of underwater navigation sensors, the method for determining the initial position based on dead reckoning (DR) trajectory translation strategy in the initial period of position was proposed to improve the accuracy of the initial position of DR system. This method combined the features of DR system and hydroacoustic navigation system, and got the optimal trajectory under the constraints of certain movement trends by translating DR trajectory to calculate the initial position. One real sea trial data was used to analyze this method. The results show that the position error of this method is 60.2% and 77.2% of the error of the average method and the least squares fitting method. The application of this method can reduce the position error of DR system and improve the stability of DR/hydroacoustic integrated navigation system.
-
航天器在上升和再入太空阶段受到巨大的气动加热效应，为保证飞行员安全和机载仪器正常运转，需在航天器的外部铺设防热层结构[1]。目前航天器防热层结构通常采用正六边形蜂窝板结构[2-3]，通过对蜂窝结构进行防热材料灌注来实现热防护的功能[4-5]。
由于蜂窝结构和防热层材料的特殊性，国内外防热层灌注多采用人工进行，但该灌注方式效率较低，随着中国航天器的发展，球冠面的外形尺寸逐渐变大，人工灌注方式无法满足防热材料灌注的需求，因此，需要提出一种满足要求的灌注机器人。
由于球冠面外形尺寸较大，灌注装置末端需携带负载，要求灌注机器人具有较大的工作空间和较高的刚度。串联机器人因其工作空间大、操作灵活、响应速度快等优点在工业生产方面得到了广泛的应用，例如，PUMA[6]、SCARA[7]和IRB[8]等工业机器人，但串联机器人同时具有刚度小的缺点。而与传统的串联机器人相比，具有复杂结构的并联机器人具有许多明显的优势，例如，高刚度、高精度、高承载能力和误差积累小等优点[9-12]，并应用于并联机床[13-14]、喷涂机器人[15]、微操作机器人[16]和力传感器[17]等方面，但并联机器人同时具有工作空间小的缺点。因此，对于灌注机器人来说，单纯的串联机器人和并联机器人都无法完全满足灌注要求。结合以上2种机器人的优点，串并混联机器人可以最大程度地满足航天器防热材料灌注要求。
本文提出一种用于防热层灌注的串并混联机器人，该机器人的工作空间能够覆盖整个球冠面。通过采用被动支链，使得并联机构具有较高的刚度。建立了并联机构的模型，对机构进行了运动学分析，通过约束分析绘制了并联机构的工作空间，在雅可比矩阵的基础上建立了并联机构的刚度模型，利用遗传算法，以工作空间和全局刚度为指标对并联机构的参数进行多目标优化分析。优化结果大大提高了并联机构的刚度和工作空间，能够满足防热层灌注的要求。
1. 机构描述
如图 1(a)所示系统主要由3PSS-PU并联机构、圆形导轨和蜂窝转台组成，其中3PSS-PU并联机构可在圆形导轨上运动，蜂窝转台可绕自身转动, 通过沿导轨和转台的自转使得灌注机构末端操作器能够到达整个蜂窝球冠面。本文重点对3PSS-PU并联机构进行分析，如图 1(b)所示机构由定平台、动平台以及连接定平台和动平台的3个完全相同的主动支链PSS和1个被动支链PU组成，主动支链均匀的分布在定平台上，每个主动支链依次由移动(P)副、球(S)副和球(S)副组成，每个P副由直线电机驱动；被动支链由P副和虎克铰(U)副组成，U副与动平台相连接。由于每条主动支链自由度为6，对动平台不提供约束，所以动平台和中间被动支链具有相同的自由度数，即动平台具有沿x轴和y轴的转动自由度以及沿z轴的移动自由度。
如图 2所示为3PSS-PU机构运动简图，其中B1、B2、B3分别代表 3个移动副，动平台上3个球副分别由M1、M2、M3表示，与P副相连的3个球副分别由N1、N2、N3表示，固定坐标系O-XbYbZb建立在定平台的中心点O，轴线Xb沿导轨
2. 运动学分析
2.1 约束分析
由图 2可得，φi和ϕi参数之间的关系表示如下：
定义θ1和θ2分别为动平台绕x轴和y轴的转角。
因此，可得出动坐标系相对于定坐标系的旋转矩阵：
(1) 式中:RM(x, θ1)为绕x轴旋转θ1的旋转矩阵; RM(y, θ2)为绕y轴旋转θ2的旋转矩阵。
定义zm为动平台沿z轴方向上的坐标，动坐标系原点M在定坐标系下的坐标MO可以表示为
(2) Mi在动坐标系下的坐标MiM可以表示为
(3) 因此，Mi在定坐标系下的坐标MiO可以表示为
(4) 2.2 运动学反解
已知输出构件的位姿，求解输入构件的位置称为机构的运动学反解。由图 2可得，第i个支链的封闭矢量方程可以表示为
(5) 式中：
其中：si(i=1~3)为每个移动副移动的距离；xli、yli、zli分别为向量
将式(4)代入式(5)可得
(6) 式中：Axi=Rmcos ϕicos θ2; Bxi=-cos φi; Ayi=Rmcos ϕisin θ1sin θ2+Rmsin ϕicos θ1; Byi=-sin φi; Azi=-Rmcos ϕicos θ1sin θ2+Rmsin ϕisin θ1+zm; Bzi=0。
根据杆长条件可得
(7) 将式(6)代入式(7)可得
(8) 式中：
因此，由式(8)中可得出运动学反解：
(9) 2.3 雅可比矩阵
由于
(10) 对式(10)两边分别对xmio、ymio和zmio求偏导数，可得
(11) 通过对式(11)整理可得
(12) 式中：λmi=ylisin φi+xlicos φi。
对式(4)两边同时对时间求偏导数可得
(13) 式中：
联立方程式(12)和方程式(13)可得
(14) 式中：xm和ym分别为动平台沿x轴和y轴方向上的坐标；Jp为仅有3条主动支链组成机构的雅可比矩阵：
如图 3所示，建立了每个运动副的局部坐标系，因此可得
(15) 
图 3 中间被动支链各运动副的局部坐标系Figure 3. Local coordinate system of every kinematic pair of middle passive link式中：a4i为旋转矩阵A10A21…Aii-1的第3列元素，a40=[0 0 1]T，Aii-1为从第i个坐标系到第i-1个坐标系的旋转矩阵，同时位置矢量可表示为
(16) 其中：b41=[0 0 zm]T；b42=[0 0 0]T；b43=[0 0 0]T。
因此，对于中间约束支链，可得
(17) 将方程式(17)代入方程式(14)，可求出并联机构的整体雅可比矩阵：
(18) 式中：
3. 性能分析
3.1 工作空间
根据机构的特点，影响机构工作空间的主要因素有：移动副移动的距离、杆的长度以及连杆分别与定平台和动平台的夹角。其中，移动副移动距离的约束是限制机构工作空间的主要因素。
因此，移动副的相应约束可表示为
(19) 式中:simin和simax分别为移动副的最小和最大位移。
定义主动连杆与动平台之间的夹角为αi，其中杆件的半径忽略不计，故其约束条件可表示为
(20) 式中：αimin和αimax分别为αi的最小值和最大值。
定义主动连杆与定平台之间的夹角为βi，故其约束条件可表示为
(21) 式中：βimin和βimax分别为βi的最小值和最大值。
3PSS-PU并联机构结构参数和相关的变量如表 1所示，通过以下参数和以上约束条件，可计算出并联机构在笛卡儿坐标系下的工作空间，如图 4(a)所示, 图 4(b)和图 4(c)分别为工作空间的右视图和左视图。
表 1 3PSS-PU并联机构结构参数Table 1. Structural parameters of 3PSS-PU parallel mechanism参数 数值 Rm/mm 250 li/mm 220 φ1/(°) 60 ϕ1/(°) 60 simin/mm 0 simax/mm 450 αimin/(°) 50 αimax/(°) 120 βimin/(°) 50 βimax/(°) 120 
图 4 3PSS-PU并联机构的工作空间及其左、右视图Figure 4. Workspace of 3PSS-PU parallel mechanism and its right and left side views3.2 刚度模型
为了分析机构在运动过程中刚度的变化趋势，需建立刚度模型，假定整个系统为刚性，得出如下表达式[18]：
(22) 式中：
ε=[τ1 τ2 Fm]T为作用在末端操作器上的力(τ1、τ2)和力矩(Fm)向量；
(23) 式中：k11、k22和k33为纯刚度，代表动平台在同一方向上力或力矩受变形的影响，在刚度矩阵中，这3个参数最重要，即其他参数可以忽略不计，k11, k22的单位为N·mm/rad，k33的单位为N/mm。
当θ1或θ2为定值时，k22在θ2方向上随着θ2、zm或θ1、zm变化的刚度分布如图 5和图 6所示。随着zm的增大，刚度都逐渐变大，当zm=140 mm时，刚度达到最大值，当zm为常数时，刚度值不随θ2或θ1的变化而变化。
图 5 k22在θ2方向上随着θ2和zm变化的刚度分布(θ1=0.3 rad)Figure 5. Stiffness distribution of k22 along θ2 with change of θ2 and zm (when θ1=0.3 rad)
图 6 k22在θ2方向上随着θ1和zm变化的刚度分布(θ2=0.3 rad)Figure 6. Stiffness distribution of k22 along θ2 with change of θ1 and zm (when θ2=0.3 rad)图 7为zm=210 mm时，k22在θ2方向上随着θ1和θ2的变化的刚度分布图, 刚度随着θ1和θ2的增大逐渐变小，当θ1=0.35 rad时，刚度达到最小值。
图 7 k22在θ2方向上随着θ1和θ2变化的刚度分布(zm=210 mm)Figure 7. Stiffness distribution of k22 along θ2 with change of θ1 and θ2 (when zm=210 mm)4. 基于遗传算法的多目标优化
4.1 目标函数的建立
优化问题主要由目标函数和约束条件两部分组成[19-20]。目标函数主要包括2个：全局刚度和工作空间。对于全局刚度，由于刚度矩阵中对角线上的主元素k11、k22、k33分别对θ1、θ2、zm上的刚度影响比较大，主要通过这几个刚度值来衡量全局刚度的大小。对于工作空间，可用工作空间内散点的个数来更衡量其大小，避免了复杂的推导过程，同时达到了优化的目的。
全局刚度E可以被描述为
(24) 为区分2个优化目标的收敛曲线，定义以下优化目标函数：
(25) 式中：W为工作空间体积的目标函数；ei和wi分别为第i个性能指标。
结合约束条件，给出优化参数的尺寸约束范围:
(26) 同时，在优化过程中为保证刚度取值在工作空间之内，应对限制工作空间的参数进行约束
(27) 4.2 优化过程
基于遗传算法对3PSS-PU并联机构进行多目标优化，优化的各项参数为：优化代数G为100，群数为20。通过编程计算，得出图 8所示的目标函数优化曲线，图中展示了从0~100代的遗传运算的过程。由图 8(a)可看出60代以后，全局刚度值曲线趋于平稳; 图 8(b)中工作空间散点曲线从40代以后趋于平稳，此时，全局刚度和工作空间散点数都达到最大。如表 2所示，列举了第100代中20组机构的优化设计参数值。
表 2 3PSS-PU并联机构优化设计参数Table 2. Optimum design parameters of 3PSS-PU parallel mechanism组数 机构设计参数 Rm/mm li/mm φ1/rad ϕ1/rad 1 266.608 0 237.058 5 0.676 7 0.687 6 2 269.573 9 237.284 5 0.634 9 0.733 3 3 270.356 9 236.858 3 0.634 9 0.733 3 4 267.452 5 237.519 3 0.676 7 0.687 6 5 269.551 9 237.284 3 0.634 9 0.733 3 6 269.551 9 237.284 3 0.676 7 0.687 6 7 267.452 5 237.519 3 0.676 7 0.687 6 8 267.199 7 237.557 2 0.634 9 0.733 3 9 266.526 2 237.056 6 0.634 9 0.733 3 10 269.573 4 237.284 4 0.634 9 0.733 3 11 270.347 2 236.971 9 0.676 7 0.687 6 12 267.452 5 237.519 3 0.634 9 0.733 3 13 269.551 7 237.284 3 0.634 9 0.733 3 14 269.329 7 237.303 0 0.676 7 0.687 6 15 270.355 1 236.848 2 0.634 9 0.733 3 16 269.562 5 237.280 2 0.634 9 0.733 3 17 269.414 6 237.272 3 0.676 7 0.687 6 18 267.473 7 237.515 3 0.676 7 0.687 6 19 270.339 5 236.969 1 0.634 9 0.733 3 20 267.452 5 237.519 3 0.634 9 0.733 3 为了从20组设计参数中选出一组参数取值，此处采用机构体积最小的原则进行选取，该并联机构的体积公式近似表示为
(28) 由式(28)可知，当si取最大值400mm时，机构的体积最大, 此时机构的体积公式可转化为
(29) 把以上20组参数值代入式(29)对比可得，第9组参数优化值的机构体积最小，因此，取该组参数作为最终优化的设计参数值，整理后列出优化参数前后的对比值，如表 3所示, θ1=θ2=0.34 rad，zm=120 mm时，优化前后的刚度对比值如表 4所示，倍数为优化值与初始值的比值，S为散点数。
表 3 3PSS-PU并联机构优化前后结构参数对比Table 3. Comparison of initial and optimized structure parameters for 3PSS-PU parallel mechanism设计参数 初始值 优化值 Rm/mm 250 267 li/mm 220 237 φ1/rad π/3 0.63 ϕ1/rad π/3 0.73 表 4 3PSS-PU并联机构优化前后刚度参数对比Table 4. Comparison of initial and optimized stiffness parameters for 3PSS-PU parallel mechanism刚度参数 初始值 优化值 倍数 k11 1 272.95 4 023.15 3.160 k22 6.244 17×107 1.102 13×108 1.765 k33 9.851 90×105 9.015 62×106 9.151 E 6.342 81×107 1.192 33×108 1.880 S 929 775 1 069 132 1.150 通过以上对于全局刚度和工作空间的优化可以得出，优化后的机构的刚度和工作空间都变大，在一定条件下，优化后的全局刚度值是初始值的1.880倍，优化后工作空间的散点数是初始值的1.150倍。
5. 结论
1) 提出了一种用于防热层材料灌注的新型3PSS-PU并联灌注机器人，采用被动支链的约束方式，提高了机构的刚度，并通过圆形导轨增大了动平台的工作空间。
2) 建立了并联灌注机构的约束方程，进行了运动学分析，在机构参数约束条件下绘制了机构的工作空间，并在雅可比矩阵的基础上建立了机构的刚度模型。
3) 以全局刚度和工作空间为指标，基于遗传算法对机构进行多目标参数优化，并提出以工作空间内散点的数目来衡量工作空间体积大小，最终给出优化后机构尺寸参数, 为蜂窝灌注机器人应用奠定基础。
-
[1] 王淑炜,张延顺.基于罗经/DVL/水声定位系统的水下组合导航方法研究[J].海洋技术学报,2014,33(1):19-23. Wang S W,Zhang Y S.Research on the underwater integrated navigation method based on compass/DVL/hydroacoustic position system[J].Journal of Ocean Technology,2014,33(1):19-23(in Chinese). [2] Rigby P,Pizarro O,Williams S B.Towards geo-referenced AUV navigation through fusion of USBL and DVL measurements[C]//Proceedings of OCEANS 2006.Piscataway,NJ:IEEE Press,2006:1-6. [3] 冯子龙,刘健,刘开周.AUV 自主导航航位推算算法的研究[J].机器人,2005,27(2):168-172. Feng Z L,Liu J,Liu K Z.Dead reckoning method for autonomous navigation of autonomous underwater vehicles[J].Robot,2005,27(2):168-172(in Chinese) [4] Jirawimut R,Ptasinski P,Garaj V,et al.A method for dead reckoning parameter correction in pedestrian navigation system[J].IEEE Transactions on Instrumentation and Measurement,2003,52(1):209-215. [5] 万磊,李璐,刘建成,等.一种基于航位推算的水下机器人导航算法[J].中国造船,2005,45(4):77-82. Wan L,Li L,Liu J C,et al.Navigation algorithm based on dead reckoning of automatic underwater vehicle[J].Shipbuilding of China,2005,45(4):77-82(in Chinese). [6] Yang X Y,He H.GPS/DR integrated navigation system based on adaptive robust Kalman filtering[C]//Proceedings of International Conference on Microwave and Millimeter Wave Technology.Piscataway,NJ:IEEE Press,2008,4:1946-1949. [7] Chung H,Ojeda L,Borenstein J.Accurate mobile robot dead-reckoning with a precision-calibrated fiber-optic gyroscope[J].IEEE Transactions on Robotics and Automation,2001,17(1):80-84. [8] 朱维庆,朱敏,刘晓东,等.海底微地貌测量系统[J].海洋测绘,2003,23(3):27-31. Zhu W Q,Zhu M,Liu X D,et al.The survey system for detailed relief on sea floor[J].Hydrographic Surveying and Charting 2003,23(3):27-31(in Chinese). [9] 朱维庆,刘晓东,张东升,等.高分辨率测深侧扫声纳[J].海洋技术,2005,24(4):29-35. Zhu W Q,Liu X D,Zhang D S,et al.High resolution bathymetric sidescan sonar[J].Ocean Technology,2005,24(4):29-35(in Chinese). [10] 孙树民,李悦.浅谈水下定位技术的发展[J].广东造船,2004(4):19-24. Sun S M,Li Y.The development of underwater positioning technology[J].Shipbuilding of Guangdong,2004(4):19-24(in Chinese). [11] Kim K,Choi H T,Lee C M.Underwater precise navigation using multiple sensor fusion[C]//Proceedings of 2013 IEEE International Underwater Technology Symposium(UT).Piscataway,NJ:IEEE Press,2013:1-4. [12] 刘晓东,张方生,朱维庆,等.深水声学拖曳系统[J].海洋测绘,2006,25(6):37-40. Liu X D,Zhang F S,Zhu W S,et al.Towed deep water acoustics system[J].Hydrographic Surveying and Charting,2006,25(6):37-40(in Chinese). [13] 蒋青吉,肖昌荣,肖波.HiPAP100水下定位系统在海底摄像中的应用[J].海洋地质前沿,2013,29(11):62-66. Jiang Q J,Xiao C R,Xiao B.The application of HiPAP 100 underwater positioning system to the seabed video system survey[J].Marine Geology Frontiers,2013,29(11):62-66(in Chinese). [14] 李守军,包更生,吴水根.水声定位技术的发展现状与展望[J].海洋技术,2005,24(1):130-135. Li S J,Bao G S,Wu S G.A practical overview and prospect of acoustic positioning technology[J].Ocean Technology,2005,24(1):130-135(in Chinese). [15] 吴永亭,周兴华,杨龙.水下声学定位系统及其应用[J].海洋测绘,2003,23(4):18-21. Wu Y T,Zhou X H,Yang L.Underwater acoustic positioning system and its application[J].Hydrographic Surveying and Charting,2003,23(4):18-21(in Chinese). -
下载:
点击查看大图
计量
- 文章访问数: 989
- HTML全文浏览量: 104
- PDF下载量: 521
- 被引次数: 0
下载:
