一种空间柔性机械臂振动抑制算法
【专利摘要】本发明公开了一种空间柔性机械臂振动抑制算法，包含以下步骤：S1、在广义坐标系下，建立刚柔耦合机械臂系统的动力学模型；S2、对刚柔耦合机械臂系统的动力学模型中的广义变量进行分解，得到快变参数和慢变参数；S3、分别针对快变参数和慢变参数建立相应的子系统，并配置对应的控制律；S4、将快变子系统的控制律和慢变子系统的控制律进行复合，以定位刚柔耦合机械臂系统的位置并进行振动抑制。本发明通过建立刚柔耦合机械臂系统的动力学模型，并将动力学模型分解为不同尺度慢变和快变两个子系统，将平台的刚性姿态运动状态与挠性振动状态分开，分别设计控制器进行组合控制，有效解决了在轨操控柔性机械臂挠性振动控制问题。
【专利说明】
一种空间柔性机械臂振动抑制算法
技术领域
[0001] 本发明涉及在轨操控航天器姿态控制技术领域，尤其涉及一种空间柔性机械臂振 动抑制算法。
【背景技术】
[0002] 我国的航天"三步走"（载人航天一空中对接一建立空间站)战略正顺利实施，现已 完成了航天员出舱活动，下一步要实现航天器的对接和空间站的建设。像我国这样航天经 费活动并不十分充裕的发展中国家，利用空间柔性机械臂进行太空操作更为合适，在节省 巨额开支的同时又能避免不必要的人员伤亡，因此对空间柔性机械臂开展多项关键技术的 研究具有重要的理论价值和工程应用价值。
[0003] 空间柔性机械臂执行操作任务时，不仅要求控制器对各关节的位置实现高精度轨 迹跟踪，而且必须快速抑制由大柔度臂杆的柔性引发的残余振动，以减少振动衰减的等待 时间，提高机械臂的工作效率。然而，对于具有柔性臂杆的空间机械臂系统，其振动控制器 的设计面临以下几个难题:首先，由关节端输入的驱动力矩到臂杆末端的端点位置输出间 的传递函数具有非最小相位特征，特别是在关节轴上有一个正的输入而在末端有负的位移 时表现更加明显;其次，机械臂操作负载和边界条件都是时变的，振动模态参数具有时变性 和不确定性;再次，建模时的模态截断技术会引起控制溢出和观测溢出，导致剩余模态的激 励，使控制性能变坏甚至失稳。这些因素使得柔性机械臂的末端精确定位控制不易实现，若 仅把关节位置作为系统的输出量进行控制，虽然可以将传递函数转化为最小相位，但显然 无法进行振动抑制，必将引起很大的动态跟踪误差;若以系统的名义截断模型进行控制器 设计，其鲁棒性又必须得以保证。
【发明内容】
[0004] 本发明的目的在于提供一种空间柔性机械臂振动抑制算法，通过建立刚柔耦合机 械臂系统的动力学模型，并将动力学模型分解为不同尺度慢变和快变两个子系统，将平台 的刚性姿态运动状态与挠性振动状态分开，分别设计控制器进行组合控制，有效解决了在 轨操控柔性机械臂挠性振动控制问题。
[0005] 为了达到上述目的，本发明通过以下技术方案实现:一种空间柔性机械臂振动抑 制算法，其特点是，用于刚柔耦合机械臂系统中，所述的刚柔耦合机械臂系统包含依次连接 的中心体、第一柔性机械臂、第二柔性机械臂及一刚性机械臂，该空间柔性机械臂振动抑制 算法包含以下步骤：
[0006] S1、在广义坐标系下，建立刚柔耦合机械臂系统的动力学模型；
[0007] S2、对刚柔耦合机械臂系统的动力学模型中的广义变量进行分解，得到快变参数 和慢变参数；
[0008] S3、分别针对快变参数和慢变参数建立相应的子系统，并配置对应的控制律；
[0009] S4、将快变子系统的控制律和慢变子系统的控制律进行复合，以定位刚柔耦合机 械臂系统的位置并进行振动抑制。
[0010] 所述的快变参数包含柔性机械臂微振动的模态坐标。
[0011] 所述的慢变参数包含中心体的平动位移、中心体的转动姿态角以及机械臂各个关 节位移量。
[0012] 所述的步骤S1包含：
[0013] 在中心体质心位置建立本体坐标系St，选取广义坐标/=[扩<的< <]， 其中，X为中心体平动变量，物为中心体本体坐标系相对惯性坐标系的姿态角，φι为与中心 体相连的第一柔性机械臂关节旋转角度，物为第一柔性机械臂与第二柔性臂之间旋转关 节的转角，Φ3为刚性机械臂相对于第二柔性机械臂的旋转角度，^为第一柔性机械臂的模 态坐标，Τ2为第二柔性机械臂的模态坐标；
[0014] 设偏速度列阵为# =[< Pf]，可知 Wx =X;W. =pb=0b；fV3 =φ, =￡ll；fV4 =iv\Wi=V，\Wb =t7;W7 =ψ3 5
[0023]得到完整的动力学方程，
[0024] 其中，中心体平动方程形式表示为：
[0025] mX - AchS；xmb - - AclSf：U(p2 - Ae,S；n, + + Ae2P：t2 = Qx
[0026] 中心体转动方程形式表示为：
[0027] -S^A^X + Jib0b + IbA + ΙΒ2ηφ2 + Jb3ii, + H；^ + = Q%
[0028]第一柔性机械臂的转动方程形式表不为：
[0029] + />, + /Π Ω, + Ι12Πφ2 + /13il3 + Κτχ + Η；2τ2 = βΩ,
[0030] 第二柔性机械臂的转动方程形式表示为：
[0031] -nTSlxTA2cX+ +n rJ22n^ +UTjJl3+TiTH：lrl+n TH：2f2=Qlu
[0032] 刚性机械臂的转动方程形式表示为：
[0033] -S^Aie± + +.^0, + Ι^Πφ, + ^3Ω.3 + ￡Τ?#, + Η^?2 =?〇3
[0034]第一柔性机械臂的振动方程形式表不为：
[0035] P；TAlcX + H；l mb + + JUp, + Η；^?3 +(ll+Wll)rl+ Wl2f2 + Α,τ, = %
[0036] 第二柔性机械臂的振动方程形式表示为：
[0037] + Κ?+ β·2ΤΠ^ + //3Τ??3 + + (I2 + W2, )τ2 + A2t2 = gr,
[0038] 其中，Jbb为卫星相对整星质心的转动惯量矩阵，為._.=_^>7?&为卫星中心体的姿态角 速度，内铰处的连体坐标系已设定为<=(m)，以此类推;第一柔性机械臂、第二柔性 机械臂级一刚性机械臂之间只有一个旋转关节，即有Α=<Ω 1λ Ω2=Π 色、Π = [0 0 表示沿着Z轴旋转。
[0039] 所述的刚柔耦合机械臂系统的动力学模型的标准形式如下：
[0041]式中，Θ包含广义坐标中中心体平动和转动六个变量、三个机械臂的旋转关节角 度，q包含了二个柔性机械臂的振动坐标，Fe为与Θ变量对应的广义外力，Fq为与变量对应的 广义外力，G为各哥氏力，τ为广义模态力，K为刚性矩阵；
[0042]刚柔耦合机械臂系统的质量矩阵M(0，q)是一正定矩阵，设它的逆阵为H(0，q)，那 么上式变为：
[0044] 上式可分别写为：
[0047]定义最小刚度系数为k=min(kii)，定义μ=?Λ，引入新的变量z = kq，贝1J有q = yz， 定义#.= //·化，将新的变量代入上式得到：
[0050] 所述的步骤S2中包含：
[0052] [Mlls(0,0)]-1 = Hlls(0,〇)-Hl2s(0,〇)[H22s(0,0)]-1H21s(0,〇);
[0053] 且(7"(狄次/c/d)=((，(认次/""：、"::) = 0
[0054] 式中下标"s"表示向量处于慢变子系统，即变量在慢时标中的计算；
[0057]引入快变时标# = 〃 士，定义新的状态变量zfl Z Zs，S,2 ~ ，式中下f 表,J、 变量处于快变子系统，于是式(1)可变换为：
[0061 ]由于慢变时标与快变时标相互独立，且在边界层的区域μ-O内慢变分量可视为常 数(即灰/ # = = 0)，此时，如果令μ = 〇，得到：
[0063] 综合状态变量Zfl及上式，可得到以状态方程形式描述的快变子系统为：
[0064] Zf=Afzf +BfTf
[0065]其中，
[0069] Tf为快变子系统的控制输入，用于抑制柔性振动，" ?"为对快变时标ξ求导；刚柔 親合机械臂系统的总控制输入为T = TS+Tf;原系统的状态变量近似为θ = θ3+0(μ)，z = Zs+Zfl +〇(μ)，当μ充分小时，可忽略高阶无穷小项〇(μ)。
[0070] 所述的步骤S3中配置慢变子系统的控制律包含：
[0071] 设慢变子系统的动力学如下形式：
[0072] Bs=M^(-Fes+^
[0073] 定义跟踪误差为：
[0074] e = 9s-0d
[0075] 定义控制量为：
[0076] ts： = Mlu (~Kte ~ K2e) + + Mm&，i
[0077] 其中，KjPK2为正定对角阵，可得动力学模型：
[0078] e + K.e + K.e^O
[0079 ]式中各分量解耦，通过选取KjPK2，可以满足控制需求。
[0080]所述的步骤S3中配置快变子系统的控制律包含：
[0081 ]构造最优控制性能指标函数：
[0083]其中，Q为半正定加权对称矩阵，R为正定加权对称矩阵，于是，快变子系统的最优 控制律可设计为：
[0084] tf = -Κ ,ζ, = - R 'B\ Pz,
[0085] 并且P为Ricatti方程的解：
[0086] P!, + 4;Ρ-Ρβ?,Ρ + ?)=〇
[0087] 所述的快变子系统采用线性二次型控制。
[0088] 所述的慢变子系统采用非线性PID控制。
[0089] 本发明一种空间柔性机械臂振动抑制算法与现有技术相比具有以下优点:应用奇 异摄动理论将系统分解成快慢两个独立的子系统，简化了控制器的设计计算，实现了对两 个子系统控制器的独立设计，通过两个控制器的组合得到整个系统的控制输入;采用组合 控制方法能够较好地改善系统的稳定控制性能，有效地抑制柔性机械臂的挠性振动，易于 工程实现和应用;本发明可使柔性机械臂运动过程微振动对中心体运动和机械臂各关节定 位的影响减小，保证中心刚体姿态稳定的基础上，有效地抑制了柔性机械臂的挠性振动，显 著改善系统的姿态控制性能。
【附图说明】
[0090] 图1为本发明一种空间柔性机械臂振动抑制算法的流程图。
【具体实施方式】
[0091] 以下结合附图，通过详细说明一个较佳的具体实施例，对本发明做进一步阐述。
[0092] 在轨服务柔性机械臂的中心刚体姿态动力学方程中除了有刚性中心体惯性特性 效应还有各个柔性机械臂产生的挠性特性影响耦合部分，另外还包含柔性机械臂振动部分 对中心刚体作用的累加外力矩部分。
[0093] 如图1所示，一种空间柔性机械臂振动抑制算法，用于刚柔耦合机械臂系统中，所 述的刚柔耦合机械臂系统包含依次连接的中心体、第一柔性机械臂、第二柔性机械臂及一 刚性机械臂，该空间柔性机械臂振动抑制算法包含以下步骤：
[0094] S1、在广义坐标系下，建立刚柔耦合机械臂系统的动力学模型。
[0095] 在中心体质心位置建立本体坐标系Sb，选取广义坐标ν=[Λ' ; < < <朽< W]， 其中，X为中心体平动变量，仇为中心体本体坐标系相对惯性坐标系的姿态角，_为与中心 体相连的第一柔性机械臂关节旋转角度，Φ2为第一柔性机械臂与第二柔性臂之间旋转关 节的转角，Φ3为刚性机械臂相对于第二柔性机械臂的旋转角度，^为第一柔性机械臂的模 态坐标，Τ2为第二柔性机械臂的模态坐标；
[0096] 设偏速度列阵为，=[醉Pf Wf町可知 W{ =X;W2 =φ,=ω"·,Ψ3 =φ{ =Ω1；^4 =il；W5 = φ2;Ψ6 =%.
[0103]可得刚性机械臂的各阶(11=1，2，...，7)为：
[0105]得到完整的动力学方程，
[0106] 其中，中心体平动方程形式表示为：
[0107] mX - AeiS^mb - - Ae2S^n^t - Ae3S；il3 + ΑΛΡ^ + Ae2P：r2 = Qx
[0108] 中心体转动方程形式表示为：
[0109] -SrAA + Jamb + Jb0{ + J,5n^ + Jjli: + B；^ + =
[0110] 第一柔性机械臂的转动方程形式表不为：
[0111] -SlxrAuX + JTM<ab + Jni\ + JnIl(p2 + Jl}n3 + + Η；.,τ2 = Qih
[0112] 第二柔性机械臂的转动方程形式表示为：
[0113] -nTS^TA2eX +nTJT62&b +πν 22π^2 +Πν2,Ω, +nri/：1T1 +Πγ//：2γ2 =ρΩ,
[0114] 刚性机械臂的转动方程形式表示为：
[0115] -S^.AieX. + 4：^. + 4Ω, + Ι^πφ2. + J3^3 + + = g〇s
[0116] 第一柔性机械臂的振动方程形式表不为：
[0117] PiTAicX + HU?, + //,7?, + //；[ηφ-, + //；fΩ3 + (/, + WIX )τ, + Ψ12τ2 + A(r, =
[0118] 第二柔性机械臂的振动方程形式表示为：
[0119] P7A1cX + Hl\mb + Η{τ: Ω, + Η；ι2Πφ2 + Η；???} + + (/, + Wlz )τ2 + Α,τ2 = 0Γ,
[0120] 其中，Jbb为卫星相对整星质心的转动惯量矩阵，4 = 为卫星中心体的姿态角 速度，内铰处的连体坐标系已设定为礞=氣為入)，以此类推;第一柔性机械臂、第二柔性 机械臂级一刚性机械臂之间只有一个旋转关节，即有Ω 2=Π 电、Π = [0 0 !]' 表示沿着Z轴旋转。
[0121 ]刚柔耦合机械臂系统的动力学模型的标准形式如下：
[0123] 式中，Θ包含广义坐标中中心体平动和转动六个变量、三个机械臂的旋转关节角 度，q包含了二个柔性机械臂的振动坐标，Fe为与Θ变量对应的广义外力，F q为与变量对应的 广义外力，G为各哥氏力，τ为广义模态力，K为刚性矩阵；
[0124] 刚柔耦合机械臂系统的质量矩阵M(0，q)是一正定矩阵，设它的逆阵为H(0，q)，那 么上式变为：
[0126]上式可分别写为：
[0129]定义最小刚度系数为k=min(kii)，定义μ=?Λ，引入新的变量z = kq，贝lj有q = yz， 定义# = . A：,,，将新的变量代入上式得到：
[0132] S2、对刚柔耦合机械臂系统的动力学模型中的广义变量进行分解，得到快变参数 和慢变参数;其中，快变参数包含柔性机械臂微振动的模态坐标，慢变参数包含中心体的平 动位移、中心体的转动姿态角以及机械臂各个关节位移量。
[0134] [Mlls(0,0)]-1 = Hlls(0,〇)-Hl2s(0,〇)[H22s(0,0)]-1H21s(0,〇);
[0135] 且(狄冷、"二//i)==((,(设二=0
[0136] 式中下标"s"表示向量处于慢变子系统，即变量在慢时标中的计算；
[0139]弓丨入快变时标而，定义新的状态变量zfi = z_zs，.:rn ，.式中下标"f"表示 变量处于快变子系统，于是式(1)可变换为：
[0143]由于慢变时标与快变时标相互独立，且在边界层的区域μ-0内慢变分量可视为常 数(即i/zs = =0)，此时，如果令μ = 〇,得到：
[0145] 综合状态变量zfl及上式，可得到以状态方程形式描述的快变子系统为：
[0146] zf =Α?ζ? +Β,τ,
[0147] 其中，
[0151] 为快变子系统的控制输入，用于抑制柔性振动，" ?"为对快变时标ξ求导；刚柔 親合机械臂系统的总控制输入为T = TS+Tf;原系统的状态变量近似为θ = θ3+0(μ)，z = Zs+Zfl +〇(μ)，当μ充分小时，可忽略高阶无穷小项〇(μ)。
[0152] S3、分别针对快变参数和慢变参数建立相应的子系统，并配置对应的控制律。
[0153] 配置慢变子系统的控制律包含：
[0154] 设慢变子系统的动力学如下形式：
[0155] Θ, = (~Fes +ts)
[0156] 由于所考虑的是柔性空间机械臂载体姿态与机械臂各关节铰协调运动的控制，因 此系统的控制输出为9S，定义系统的期望输出向量为9 d，定义跟踪误差为：
[0157] e = 9s-0d
[0158] 定义控制量为：
[0159 ] = [-K.fi - K2e). + 'F0s + M：ns0d
[0160] 其中，KdPK2为正定对角阵，可得动力学模型：
[0161 ] e + +
[0162 ]式中各分量解耦，通过选取KjPK2，可以满足控制需求。
[0163] 针对快变子系统，由于(Af，Bf)完全可控，因此可采用最优控制理论，即用线性二次 型最优控制器(LQR)以实现快变子系统的控制。配置快变子系统的控制律包含：
[0164] 构造最优控制性能指标函数：
[0166]其中，Q为半正定加权对称矩阵，R为正定加权对称矩阵，于是，快变子系统的最优 控制律可设计为：
[0170] S4、将快变子系统的控制律和慢变子系统的控制律进行复合，以定位刚柔耦合机 械臂系统的位置并进行振动抑制;快变子系统采用线性二次型控制，用于抑制振动，慢变子 系统采用非线性PID控制，用于提高刚体运动的鲁棒性。
[0171] 系统总的控制输入则为
[0172] τ = Ts+Tf
[0173] 由于所设计的线性二次型控制器采用的是全状态反馈的形式，而振动测量敏感器 测得的需要包括卫星本体位姿以外，还需要测量柔性机械臂振动信息。以此实际操作过程 中需要采用激光、光纤传感器等敏感器。
[0174] 尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的 描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的 多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。
【主权项】
1. 一种空间柔性机械臂振动抑制算法，其特征在于，用于刚柔禪合机械臂系统中，所述 的刚柔禪合机械臂系统包含依次连接的中屯、体、第一柔性机械臂、第二柔性机械臂及一刚 性机械臂，该空间柔性机械臂振动抑制算法包含W下步骤： 51、 在广义坐标系下，建立刚柔禪合机械臂系统的动力学模型； 52、 对刚柔禪合机械臂系统的动力学模型中的广义变量进行分解，得到快变参数和慢 变参数； 53、 分别针对快变参数和慢变参数建立相应的子系统，并配置对应的控制律； 54、 将快变子系统的控制律和慢变子系统的控制律进行复合，W定位刚柔禪合机械臂 系统的位置并进行振动抑制。2. 如权利要求1所述的空间柔性机械臂振动抑制算法，其特征在于，所述的快变参数包 含柔性机械臂微振动的模态坐标。3. 如权利要求1所述的空间柔性机械臂振动抑制算法，其特征在于，所述的慢变参数包 含中屯、体的平动位移、中屯、体的转动姿态角W及机械臂各个关节位移量。4. 如权利要求1所述的空间柔性机械臂振动抑制算法，其特征在于，所述的步骤S1包 含： 在中屯、体质屯、位置建立本体坐标系Sb，选取广义坐标扳衫片巧运括]，其 中，X为中屯、体平动变量，化为中屯、体本体坐标系相对惯性坐标系的姿态角，斬为与中屯、体 相连的第一柔性机械臂关节旋转角度，带2为第一柔性机械臂与第二柔性臂之间旋转关节的 转角，毕3为刚性机械臂相对于第二柔性机械臂的旋转角度，τι为第一柔性机械臂的模态坐 标，Τ2为第二柔性机械臂的模态坐标； 设偏速度列阵为[听' 岭r/ W/' r/的]，可知 巧=灰巧:=耗=码;K =病町=亡1;狀5 =碱;K =知而=柄=〇3 . 利用关系得到，中屯、体的各阶运'* (11 = 1，2，..:.，7)为：可得第一柔性机械臂的各阶每* (虹=1，2，...,7)为：可得第二柔性机械臂的各阶( h = 1，2,..,，7)为：得到完整的动力学方程， 其中，中屯、体平动方程形式表示为：其中，Jbb为卫星相对整星质屯、的转动惯量矩阵，或=瓜辟为卫星中屯、体的姿态角速度， 内较处的连体坐标系已设定为皆=战此类推;第一柔性机械臂、第二柔性机械臂 级一刚性机械臂之间只有一个旋转关节，即有。i=a濟、化=Π 拓、Π = [0 0 1]/表示沿 着Ζ轴旋转。5. 如权利要求4所述的空间柔性机械臂振动抑制算法，其特征在于，所述的刚柔禪合机 械臂系统的动力学模型的标准形式如下：式中，Θ包含广义坐标中中屯、体平动和转动六个变量、Ξ个机械臂的旋转关节角度，q包 含了二个柔性机械臂的振动坐标，Fe为与Θ变量对应的广义外力，Fq为与变量对应的广义外 力，G为各哥氏力，τ为广义模态力，K为刚性矩阵； 刚柔禪合机械臂系统的质量矩阵M(0，q)是一正定矩阵，设它的逆阵为H(0，q)，那么上 式变为：定义最小刚度系数为k = min化ii)，定义μ=?Λ，引入新的变量z = kq，则有q =化，定义 兵=,"· /("，将新的变量代入上式得到：6. 如权利要求5所述的空间柔性机械臂振动抑制算法，其特征在于，所述的步骤S2中包 含： 令μ充分小，则公??〇简化为： [Mils(目，0)]-1 =出 Is(目，0)-出 2s(目，0)[出 2s(目，0)]-1 出 Is(目，0); 且(?"(庆分,"?)二(7。(化灰二 0 式中下标V'表示向量处于慢变子系统，即变量在慢时标中的计算；引入快变时标《=f / '如，定义新的状态变量Zf 1 = Z-Zs，5：.。= .，式中下标吁"表示变量 处于快变子系统，于是式(1 )可变换为：其中，式(1)表示为：由于慢变时标与快变时标相互独立，且在边界层的区域μ^ο内慢变分量可视为常数 (即?/ζ, / 运,=0)，此时，如果令μ = 〇,得至Ij:综合状态变量Zfi及上式，可得到W状态方程形式描述的快变子系统为：Tf为快变子系统的控制输入，用于抑制柔性振动，"?"为对快变时标ξ求导；刚柔禪合机 械臂系统的总控制输入为τ = τs+τf;原系统的状态变量近似为θ = θs+o(μ)，z = Zs+Zfl+o(μ)， 当μ充分小时，可忽略高阶无穷小项〇(μ)。7. 如权利要求6所述的空间柔性机械臂振动抑制算法，其特征在于，所述的步骤S3中配 置慢变子系统的控制律包含： 设慢变子系统的动力学如下形式：定义跟踪误差为： e =目S-目d 定义控制量为：其中，Κι和K2为正定对角阵，可得动力学模型：式中各分量解禪，通过选取Κι和拉，可W满足控制需求。8. 如权利要求7所述的空间柔性机械臂振动抑制算法，其特征在于，所述的步骤S3中配 置快变子系统的控制律包含： 构造最优控制性能指标函数：其中，Q为半正定加权对称矩阵，R为正定加权对称矩阵，于是，快变子系统的最优控制 律可设计为： tf=-K,:.,=-R、B[Pz, 并且P为化catti方程的解：9. 如权利要求1所述的空间柔性机械臂振动抑制算法，其特征在于，所述的快变子系统 采用线性二次型控制。10. 如权利要求1所述的空间柔性机械臂振动抑制算法，其特征在于，所述的慢变子系 统采用非线性PID控制。
【文档编号】G05B13/04GK106094528SQ201610549912
【公开日】2016年11月9日
【申请日】2016年7月13日
【发明人】孙宏丽, 卢山, 侯月阳, 孙玥, 孙禄君, 王奉文
【申请人】上海航天控制技术研究所