基于MPSP算法的自由漂浮空间机器人路径规划.pdf 10页

'中国科技论文在线http://www.paper.edu.cn基于MPSP算法的自由漂浮空间机器人路径#规划11112**武耀发，宁昕，岳晓奎，袁建平，张敬敏5（1.西北工业大学航天飞行动力学技术国家重点实验室；2.中国兵器工业第二〇八研究所）摘要：由于自由漂浮空间机器人系统的非完整特性，使得我们不能像地面固定基座机器人一样通过逆运动学的方法对其进行路径规划，而且传统的自由漂浮空间机器人路径规划方法并10不能很好地考虑控制力矩的大小。因此，本文针对自由漂浮空间机器人的路径规划问题，提出了基于MPSP（模型预测静态规划）算法的自由漂浮空间机器人路径规划方法。首先，通过齐次坐标转换和拉格朗日法分别建立了自由漂浮空间机器人的运动学模型和动力学模型，在此基础上推导得到了系统的状态空间模型，给定初始状态以及末端期望位姿，选取关节力矩最小作为性能指标，将自由漂浮空间机器人的路径规划问题转化为带有终端约束的优化问15题。然后，应用MPSP算法，将PD控制产生的控制量作为初始猜想，并利用最优控制的原理推导得到了控制偏差与末端位置误差之间的函数关系，通过对控制量的不断修正和状态方程的迭代求解，使得末端到达期望位置。最后，以平面2DOF自由漂浮空间机器人为例进行数值仿真。仿真结果表明，应用MPSP算法对自由漂浮空间机器人进行路径规划，得到的关节轨迹平滑，并且在满足末端位置约束的同时，还能将关节力矩减小。20关键词：自由漂浮空间机器人；动力学模型；MPSP；路径规划中图分类号：V42PathPlanningforFree-floatingSpaceRobotBasedonMPSPAlgorithm1111225WuYaofa,NingXin,YueXiaokui,YuanJianping,ZhangJingmin(1.ScienceandTechnologyonAerospaceFlightDynamicsLaboratory,NorthwesternPolytechnicalUniversity;2.NO.208ResearchInstituteofChinaOrdnanceIdustries)Abstract:DuetothenonholonomicnatureofFFSR(free-floatingspacerobot),itisunabletoperforma30pathplanningforFFSRasafixed-baserobotthroughinversekinematics,andthetraditionalpathplanningmethodsforFFSRdon’tpreferablyconsiderthefollow-upcontroltorquerequired.Therefore,thispaperproposesapathplanningmethodbasedonmodel-predictionstaticprogramming(MPSP)forFFSR.Firstly,thekinematicmodelofFFSRisderivedfromthemethodofhomogeneouscoordinatetransformation,andthedynamicmodelofFFSRisestablishedbyLagrangianmethod.Thepath35planningproblemofFFSRisconsideredastheoptimizationproblemwithterminalconstraintswhenthedesiredendpositionwassetandthetorquewasselectedasperformanceindex.Then,theMPSPalgorithmareintroducedtosolvetheoptimizationproblem.ThecontrolhistoryproducedbyPDcontrolistakenastheinitialguess,andtherelationshipbetweenthecontroldeviationandtheendpositionerrorisderivedbytheprincipleofoptimalcontrol.Sotheendcanreachthedesiredposition40throughconstantmodificationofthecontrolhistoryanditerativelysolutionofthestateequation.Finally,thenumericalsimulationiscarriedoutwithaplanar2-DOFFFSRasanexample.Thesimulationresultsshowthattheresultedjointtrajectoryaresmooth,andthetorquecanbereducedby50%atthesametimewhentheconstraintofendpositionwassatisfied.Keywords:FFSR;dynamicmodel;MPSP;pathplanning45基金项目：国家自然科学基金（11402200）；教育部博士点基金；航天科技创新基金（GSR1572001）；航天技术支撑基金（2015-HT-XGD）作者简介：武耀发（1991-），男，硕士研究生，主要研究方向：空间机器人动力学与控制通信联系人：宁昕（1982-），男，副教授，破格博士生导师，主要研究方向：复杂空间系统设计、动力学建模与控制等.E-mail:ningxin@nwpu.edu.cn-1- 中国科技论文在线http://www.paper.edu.cn0引言随着空间科学技术的发展，人类的太空活动日益频繁。然而，超低温、微重力、强辐射的太空环境对宇航员的生命安全构成了巨大的威胁，再加上太空任务高难度、高精度的要求，50更是对宇航员的一大挑战。因此，用空间机器人代替宇航员执行一系列太空任务便成了必然[1,2]趋势。空间机器人有两种工作模式，分别为自由飞行和自由漂浮。由于在自由漂浮工作模式下比自由飞行工作模式消耗更少的燃料，因此自由漂浮空间机器人也就成了研究的重点。与地面固定基座机器人不同，自由漂浮空间机器人机械臂的运动会引起基座的运动，因此具有较强的动力学耦合特性，为非完整约束系统。这使得自由漂浮空间机器人的运动学与55动力学建模、路径规划以及跟踪控制变得非常困难。尤其是路径规划一直以来都是学者们研[3,4]究的热点。[5]MPSP（模型预测静态规划）算法最早是Padhi和Kothari于2009正式提出的，目的是解决带有终端约束的有限时域最优控制问题。MPSP算法结合了模型预测控制（MPC）和近似动态规划法（ADP）的思想，将动态优化问题转化为静态优化问题来解决，它的主要优点60是适用于任何非线性系统，并且计算效率非常高。Maity和Oza等人又提出了一种广义模型[6]预测静态规划算法（GMPSP），并将其成功应用于带有角度约束的空对地导弹的制导设计。由于MPSP算法是近几年提出的，因此国内关于MPSP算法的研究与应用不是很多。杨海[7]澜提出了一种基于MPSP算法的多约束非线性制导方法，该方法能很好地满足末端位移、末端弹道倾角、弹道偏角和法向加速度约束。李新三等人针对带有末端多约束的非线性制导65问题，运用GMPSP算法设计了一种快速求解连续时间系统具有终端落角约束的非线性最优[8]制导律。本文将针对自由漂浮空间机器人的路径规划问题进行研究，通过建立自由漂浮空间机器人的运动学和动力学模型，在给定系统初始状态以及末端期望位姿的情况下，选取关节力矩最小作为性能指标，将自由漂浮空间机器人的路径规划问题转化为带有终端约束的优化问题。然后，应用MPSP算法进行求解，求解过程中的初始猜想由PD控制提供。最后将70通过数值仿真来验证MPSP算法应用于自由漂浮空间机器人路径规划的有效性。1空间机器人动力学建模1.1运动学模型空间机器人系统示意图如图1所示。图中O表示系统质心，O表示基座质心，Occ0i3（i0,1,2,）为两根连杆之间的关节，O（i1,2,）表示连杆质心，rR为基座ci03375质心位置矢量，rR（i1,2,）为连杆质心位置矢量，pR（i1,2,）为关节ii3位置矢量，pR为末端位置矢量，b为基座质心到第一个关节的位置矢量，ae0i（i1,2,）为关节到连杆质心的位置矢量，b（i1,2,）为连杆质心到下一关节的位i-2- 中国科技论文在线http://www.paper.edu.cn置矢量。80图1空间机器人系统示意图Fig.1Sketchofspacerobotsystem从图中可以看出，空间机器人末端位置矢量可以表示为如下的形式：nnper0biai(1)ii01上式即为空间机器人系统位置级正向运动学方程，其中所有矢量都是在惯性系下表示的，对85于自由漂浮空间机器人来说，系统质心位置不变，因此一般情况下可以将质心系视为惯性系。不难发现，直接应用式（1）求取末端位置矢量比较繁琐。因此，我们一般是通过齐次坐标转换的方法求取末端位置矢量的，这种方法比较容易。一般来说，齐次坐标变换矩阵的形式如下：axbxcxpxOORPabcpOAAyyyyT(2)A01abcpzzzz0001OO90上式中，R表示刚体的姿态旋转矩阵，P表示刚体质心位置矢量。在已知各连杆DH参AA数的情况下，可以求出每根连杆相对于前一连杆的转换矩阵，从而可以得到末端相对于基座0坐标系的转换矩阵T，如下式所示：n001nn21TTTTT(3)n12n1ni-1上式中，T（in1,2,）表示连杆i相对于连杆i1的齐次转换矩阵。i951.2动力学模型对于自由漂浮空间机器人系统来说，机械臂与基座存在严重的动力学耦合，这给系统的动力学建模及控制带来了较大的困难。因此，我们采用类似于研究地面固定基座机器人的方法，将系统基座看成一个6自由度的虚拟连杆，来建立自由漂浮空间机器人系统的动力学模型。100定义系统的广义坐标q[]qq，其中q为基座部分的广义坐标，q为连杆部分的广smsm义坐标，q[]qqq，则自由漂浮空间机器人系统的总动能如下：m12n-3- 中国科技论文在线http://www.paper.edu.cnn11TTTT(iIiimiirri)qMqq()(4)22i0上式中，m，I，分别为系统各部分的质量、惯量以及角速度，Mq()为系统的惯量矩iii阵，其表达式如下（这里不作详细推导）：ABCT105Mq()BEF(5)TTCFN式中，A，B，C，E，F和N的表达式如下：nnAmmi,,Bir0gr0gr0rgii00nTEI0Iimiirr0i0,ri0rir0i1nnFIJiRimmiirJ0Ti,CiJTiii10nTTNJIJRiiRimiJJTiTii1J[k(pp),,,k(pp)]Ti11enenJ[,k,,0,k,0]Ri1i上式中，k表示第i个关节转轴的单位方向矢量。i由于太空环境为微重力环境，因此我们可将系统势能忽略不计，则自由漂浮空间机器人110系统的拉格朗日动力学方程如下：dTTQ(6)dtqq式中Q为系统的广义力，由于我们研究的对象为自由漂浮空间机器人，因此基座不受任何外力以及外力矩，因此，Q=[]0τ，τ为关节力矩。将式（4）代入式（7）可得系统的动力学模型如下：115MqqCqqqQ()(,)(7)式中Cqq(,)为系统的科氏力矩阵，可由系统惯量矩阵Mq()求得，具体表达式如下：nddmm1ijjkCij(8)k1ddqqki2式中，mMq()，ijk,,1,,n。ij一般连续系统的状态方程如下：120xAxBu(9)对于自由漂浮空间机器人系统，令xq，xq，则由式（8）可得系统的状态方程12如下：11xx11MC0MQ(10)xx22I00式中，I为单位矩阵，0为零矩阵。-4- 中国科技论文在线http://www.paper.edu.cn125对于自由漂浮空间机器人而言，其系统的状态矩阵A和B是时变的，为了简化计算，可近似认为在仿真步长这一小段时间内系统状态矩阵A与B是不变的。令系统的输出为yp，并选取如下的性能指标：e1TJ=(11)2不难看出，当我们给定末端期望位置p时，自由漂浮空间机器人的路径规划问题就可e130以看作带有末端位置约束的路径优化问题。下面我们将介绍如何应用MPSP算法进行该问题的求解。2MPSP算法2.1算法原理MPSP算法是结合了模型预测控制（MPC）和近似动态规划法（ADP）思想的用来求解135带有终端约束的最优控制问题的一种算法。MPSP算法的基本原理如下：为了应用MPSP算法，需将连续系统离散化处理。对于如下形式的连续系统：xfxu(,)(12)yhx()采用欧拉迭代法进行离散化处理可得系统离散形式的状态方程和输出方程：XFXU(,)XfXU(,)t(13)k1kkkkk140YhX()(14)kknmp式中，XR，UR，YR，kN12，，，表示离散的时间节点，t表示时间步长。MPSP算法的目标就是找出合适的控制历程U（需要一个恰当的初始猜想），使得k终端时刻的实际输出Y趋于期望输出Y，即YY，同时在该过程中使得控制量尽可NNNN能小。145将Y在Y处一阶Taylor展开可得：NNYNYYdX()X(15)NNNNXN式中，()X为高阶项。由上式可得系统的输出误差为：NYNYddYX(16)NNNXN由式（13）可得k1时刻系统的状态误差为：FFkk150dXdXdU(17)k1kkXUkk式中，dX和dU分别为系统k时刻的状态误差和控制偏差。kk令式（17）中的kN1，然后将其代入式（16）得：YFFNN11NdYdXdU(18)NN11NXXUNN11N同理，令kN2，重复上述过程直至k1，可得：-5- 中国科技论文在线http://www.paper.edu.cn155dYAXdBUdBdU(19)N111N1N1式中，A，B的表达式如下：kYFFNN11NAXXXNN11YFFFNN11kkBkXXXUNN11kk由于初始状态已知，式（19）中状态误差的第一项dX0，因此，式（19）变为：1N1160ddYNBUkk(20)k10我们注意到权重矩阵B是可以通过迭代计算的，首先，定义B如下式：kN10YNB(21)N1XN0然后计算B，kN2,N3,,1，如下：k00Fk1BB(22)kk1Xk1165最后计算B，kN2,N3,,1，如下：k0FkBB(23)kkUk在方程组（20）中共有(Nm1)个未知量和p个方程，一般来说，p(N1)m，因此，该方程组有多组解，可保证从中取得最优解。我们给出如下的性能指标：N1100TJ(UkdUk)RUk(kdUk)(24)2k10170式中，U为当前控制量，R为权重矩阵。kk方程（20）和（24）形成了一个带有约束的静态优化问题。利用最优控制的原理，可得该问题的哈密尔顿函数为：N1THJdYNBUkdk(25)k1式中，为拉格朗日乘子。要使得性能指标达到最优，需满足下面的两个条件：H0T175RU()dUB0(26)kkkkdUkN1H=dYNBUkdk0(27)k1求解式（26）可得：10TdURBU(28)kkkk将式（28）代入式（27），可得：180AbdY(29)Nppp1式中，AR，bR，它们的表达式如下：-6- 中国科技论文在线http://www.paper.edu.cnNN1110TABRBkkk,bBUkkkk11联立式（28）与式（29）消去，可得控制偏差如下：1T10ddURBA()YbU(30)kkkNk185因此，更新后的控制量如下：01T1UUddURBA()Yb(31)kkkkkN2.2算法流程MPSP算法主要包含预测和修正两个步骤，第一次迭代的预测值由初始猜想提供，之后经过修正作为下一次迭代的预测值，迭代终止的条件是实际输出与期望输出的差值满足设定190值。MPSP算法实施的具体流程如图2所示。图2MPSP算法流程图Fig.2FlowchartofMPSPalgorithm3仿真算例1953.1模型参数设置为了验证MPSP算法应用于自由漂浮空间机器人路径规划的有效性，我们对一个平面2DOF自由漂浮空间机器人进行仿真。机器人的参数设置如表1所示。图表的标注：图片标注在下方，表格标注在表格上方，居中，汉字用宋体，数字英文用TimesNewRoman,罗马字用Symbol。字号：小五。200表1自由漂浮空间机器人系统几何参数Tab.1GeometricparametersofFFSRBodyBaseLink1Link2-7- 中国科技论文在线http://www.paper.edu.cnMass/(kg)12.94.51.52Inertia/(kgm)0.2080.320.049Length/(m)0.3270.620.6为了验证MPSP算法在不同情况下的收敛特性，我们给出了两个算例，这两个算例有不同的初始状态和期望末端位置，具体参数如表2所示。在这两个算例中，仿真时间均为5s，*2仿真步长均为0.05s，算法迭代的终止条件均设为YY10。NN205表2不同的初始条件和期望值设定Tab.2SettingfordifferentinitialconditionsanddesiredvalueCaseInitialstateDesiredpositionCase1q=[-0.22-0.01902.14-2.71]pe=[0.560.41]Case2q=[-0.13-0.1001.43-2.48]pe=[0.820.24]3.2初始猜想我们采用简单的PD控制来产生MPSP算法所需的控制量的初始猜想值。针对本文研究的问题，我们设计如下形式的PD控制律：210Kq()qKq(32)pdd式中，为关节控制力矩，K和K分别为比例增益和微分增益（仿真时取K1.2，pdpK2），q为期望关节角，可由期望末端位置通过逆运动学求解获得。dd3.3仿真结果及分析算例1的迭代次数为16次，仿真结果如图3~6所示。从图3可以看出，用MPSP算法215规划出的轨迹平滑且末端能到达期望位置。从图4和图5中可以看出，用MPSP算法规划得到的基座姿态和关节角一开始变化比较平缓，这说明MPSP算法在仿真初期对预测控制量的修正较为明显。而到仿真后期时，MPSP算法对应的基座姿态和关节角的变化比较剧烈，这对路径规划来说是不利的。从图6可以看出，用MPSP算法进行路径规划所需的控制力矩大幅度减小（尤其是在仿真初期），这正是MPSP算法优势的体现。220图3机械臂末端轨迹（case1）图4基座姿态变化曲线（case1）Fig.3Trajectoryofend-effectorforcase1Fig.4Variationofbaseattitudeforcase1-8- 中国科技论文在线http://www.paper.edu.cn图5各关节角的变化曲线（case1）图6各关节力矩的变化曲线（case1）225Fig.5Variationofjointangleforcase1Fig.6Variationofjointtorqueforcase1算例2的迭代次数为8次，仿真结果如图7~10所示。从图8可以看出，用MPSP算法规划得到的基座姿态变化较大，这一现象可以用图9中MPSP算法对应的关节角的变化来解释，从图9可以看出MPSP算法对应的关节角均为增大趋势，因此与之对应的关节角速度均为正，由角动量守恒原理可知，基座姿态角速度为负且绝对值较大，因此基座姿态的变化较230大。此外，从图9中可以看出，在算例2中，MPSP算法与PD控制对应的关节角的变化相近，这是因为在算例2中，PD控制收敛较快，初始猜想已经很接近最优解，因此MPSP算法对预测控制量的修正作用不是很明显。图7机械臂末端轨迹（case2）图8基座姿态变化曲线（case2）235Fig.7Trajectoryofend-effectorforcase2Fig.8Variationofbaseattitudeforcase2图9各关节角的变化曲线（case2）图10各关节力矩的变化曲线（case2）Fig.9Variationofjointangleforcase2Fig.10Variationofjointtorqueforcase2-9- 中国科技论文在线http://www.paper.edu.cn2404结论本文应用MPSP算法对自由漂浮空间机器人进行了路径规划方面的研究。首先，建立了自由漂浮空间机器人的运动学模型和动力学模型，并在此基础上，推导得到了系统的状态空间模型。在给定初始状态以及末端期望位姿的情况下，选取关节力矩最小作为性能指标，将自由漂浮空间机器人的路径规划问题转化为带有终端约束的优化问题。然后，应用MPSP245算法对该优化问题进行求解，求解过程中将PD控制产生的控制量作为初始猜想，通过对预测控制量的不断修正来满足末端位置约束。最后，通过数值仿真验证了该算法的有效性，从而为非完整系统的路径规划提供了一种新的思路。虽然MPSP算法有诸多优势，但其作为次优规划算法，其收敛特性还有待研究。另外，一般来说，在空间机器人的路径规划过程中会引入各种约束，如避障、避奇异、避关节极限250等，这会使得规划问题的求解变得非常困难，因此，研究考虑状态约束的MPSP算法在空间机器人路径规划中的应用是十分有必要的。致谢感谢国家自然基金以及教育部博士点基金等项目对本文的支持和帮助。[参考文献](References)255[1]张文辉，叶晓平，季晓明.国内外空间机器人技术发展综述[J].飞行力学，2013，31（3）：198-202.[2]梁斌，杜晓东，李成.空间机器人非合作航天器在轨服务研究进展[J].机器人，2012，34（2）：242-256.[3]谢国伟.空间机器人的运动控制研究[D].哈尔滨：哈尔滨工业大学，2006.[4]徐文福，强文义，李成.自由漂浮空间机器人路径规划研究进展[J].哈尔滨工业大学学报，2009，41（11）：1-12.260[5]PadhiR,KothariM.Modelpredictivestaticprogramming:acomputationallyefficienttechniqueforsuboptimalcontroldesign[J].Internationaljournalofinnovativecomputing,informationandcontrol,2009,5(2):399-411.[6]MaityA,OzaHB,PadhiR.Generalizedmodelpredictivestaticprogrammingandangle-constrainedguidanceofair-to-groundmissiles[J].JournalofGuidanceControlandDynamics,2014,37(6):1897-1913.265[7]杨海澜.基于MPSP算法的多约束非线性制导律研究[J].计算机仿真，2016，33（1）：38-41.[8]李新三，汪立新，丁邦平，等.基于G-MPSP算法的非线性制导律研究[J].弹箭与制导学报，2016，36（2）：1-5.-10-'