本发明公开了一种空间柔性机械臂振动抑制算法，包含以下步骤：S1、在广义坐标系下，建立刚柔耦合机械臂系统的动力学模型；S2、对刚柔耦合机械臂系统的动力学模型中的广义变量进行分解，得到快变参数和慢变参数；S3、分别针对快变参数和慢变参数建立相应的子系统，并配置对应的控制律；S4、将快变子系统的控制律和慢变子系统的控制律进行复合，以定位刚柔耦合机械臂系统的位置并进行振动抑制。本发明通过建立刚柔耦合机械臂系统的动力学模型，并将动力学模型分解为不同尺度慢变和快变两个子系统，将平台的刚性姿态运动状态与挠性振动状态分开，分别设计控制器进行组合控制，有效解决了在轨操控柔性机械臂挠性振动控制问题。
1.一种空间柔性机械臂振动抑制算法，其特征在于，用于刚柔耦合机械臂系统中，所述的刚柔耦合机械臂系统包含依次连接的中心体、第一柔性机械臂、第二柔性机械臂及一刚性机械臂，该空间柔性机械臂振动抑制算法包含以下步骤：S1、在广义坐标系下，建立刚柔耦合机械臂系统的动力学模型；
S2、对刚柔耦合机械臂系统的动力学模型中的广义变量进行分解，得到快变参数和慢变参数；
S3、分别针对快变参数和慢变参数建立相应的子系统，并配置对应的控制律；
S4、将快变子系统的控制律和慢变子系统的控制律进行复合，以定位刚柔耦合机械臂系统的位置并进行振动抑制；
所述的刚柔耦合机械臂系统的动力学模型的标准形式如下：式中，M(θ,q)为刚柔耦合机械臂系统的质量矩阵，θ包含广义坐标中中心体平动和转动六个变量、三个机械臂的旋转关节角度，q包含了二个柔性机械臂的振动坐标，Fθ为与θ变量对应的广义外力，Fq为与q变量对应的广义外力，G为各哥氏力，Gθ为与θ变量对应的哥氏力，Gq为与q变量对应的哥氏力；τ为广义模态力，K为刚性矩阵，Kq为q变量对应的刚性矩阵。
2.如权利要求1所述的空间柔性机械臂振动抑制算法，其特征在于，所述的快变参数包含柔性机械臂微振动的模态坐标。
3.如权利要求1所述的空间柔性机械臂振动抑制算法，其特征在于，所述的慢变参数包含中心体的平动位移、中心体的转动姿态角以及机械臂各个关节位移量。
4.如权利要求1所述的空间柔性机械臂振动抑制算法，其特征在于，所述的步骤S1包含：在中心体质心位置建立本体坐标系Sb，选取广义坐标其中，X为中心体平动变量， 为中心体本体坐标系相对惯性坐标系的姿态角， 为与中心体相连的第一柔性机械臂关节旋转角度， 为第一柔性机械臂与第二柔性臂之间旋转关节的转角， 为刚性机械臂相对于第二柔性机械臂的旋转角度，τ1为第一柔性机械臂的模态坐标，τ2为第二柔性机械臂的模态坐标；
利用 关系得到，中心体的各阶 h＝1,2,…,7为：可得第一柔性机械臂的各阶 为：
第一柔性机械臂的转动方程形式表示为：第二柔性机械臂的转动方程形式表示为：刚性机械臂的转动方程形式表示为：
第一柔性机械臂的振动方程形式表示为：第二柔性机械臂的振动方程形式表示为：其中，Jbb为卫星相对整星质心的转动惯量矩阵， 为卫星中心体的姿态角速度，以此类推；第一柔性机械臂、第二柔性机械臂级一刚性机械臂之间只有一个旋转关节，即有Π＝[0 0 1]′表示沿着Z轴旋转。
5.如权利要求4所述的空间柔性机械臂振动抑制算法，其特征在于，刚柔耦合机械臂系统的质量矩阵M(θ,q)是一正定矩阵，设它的逆阵为H(θ,q)，那么刚柔耦合机械臂系统的动力学模型的标准形式变为：上式可分别写为：
定义最小刚度系数为k＝min(kii)，定义μ＝1/k，引入新的变量z＝kq，则有q＝μz，定义将新的变量代入上式得到：
6.如权利要求5所述的空间柔性机械臂振动抑制算法，其特征在于，所述的步骤S2中包含：令μ充分小，则公式 可简化为：
[M11s(θ,0)] ＝H11s(θ,0)-H12s(θ,0)[H22s(θ,0)] H21s(θ,0)；
式中下标“s”表示向量处于慢变子系统，即变量在慢时标中的计算；
引入快变时标 定义新的状态变量zf1＝z-zs， 式中下标“f”表示变量处于快变子系统，于是式(1)可变换为：其中，式(1)表示为：
由于慢变时标与快变时标相互独立，且在边界层的区域μ→0内慢变分量可视为常数，即 此时，如果令μ＝0，得到：综合状态变量zf1及上式，可得到以状态方程形式描述的快变子系统为：其中，
τf为快变子系统的控制输入，用于抑制柔性振动，“·”为对快变时标ξ求导；刚柔耦合机械臂系统的总控制输入为τ＝τs+τf；原系统的状态变量近似为θ＝θs+o(μ)，z＝zs+zf1+o(μ)，当μ充分小时，可忽略高阶无穷小项o(μ)；
所述的步骤S3中配置慢变子系统的控制律包含：设慢变子系统的动力学如下形式：
其中，K1和K2为正定对角阵，可得动力学模型：式中各分量解耦，通过选取K1和K2，可以满足控制需求。
7.如权利要求6所述的空间柔性机械臂振动抑制算法，其特征在于，所述的步骤S3中配置快变子系统的控制律包含：构造最优控制性能指标函数：
其中，Q为半正定加权对称矩阵，R为正定加权对称矩阵，于是，快变子系统的最优控制律可设计为：并且P为Ricatti方程的解：
8.如权利要求1所述的空间柔性机械臂振动抑制算法，其特征在于，所述的快变子系统采用线性二次型控制。
9.如权利要求1所述的空间柔性机械臂振动抑制算法，其特征在于，所述的慢变子系统采用非线性PID控制。
一种空间柔性机械臂振动抑制算法
技术领域
[0001] 本发明涉及在轨操控航天器姿态控制技术领域，尤其涉及一种空间柔性机械臂振动抑制算法。
背景技术
[0002] 我国的航天“三步走”(载人航天一空中对接一建立空间站)战略正顺利实施，现已完成了航天员出舱活动，下一步要实现航天器的对接和空间站的建设。像我国这样航天经费活动并不十分充裕的发展中国家，利用空间柔性机械臂进行太空操作更为合适，在节省巨额开支的同时又能避免不必要的人员伤亡，因此对空间柔性机械臂开展多项关键技术的研究具有重要的理论价值和工程应用价值。
[0003] 空间柔性机械臂执行操作任务时，不仅要求控制器对各关节的位置实现高精度轨迹跟踪，而且必须快速抑制由大柔度臂杆的柔性引发的残余振动，以减少振动衰减的等待时间，提高机械臂的工作效率。然而，对于具有柔性臂杆的空间机械臂系统，其振动控制器的设计面临以下几个难题：首先，由关节端输入的驱动力矩到臂杆末端的端点位置输出间的传递函数具有非最小相位特征，特别是在关节轴上有一个正的输入而在末端有负的位移时表现更加明显；其次，机械臂操作负载和边界条件都是时变的，振动模态参数具有时变性和不确定性；再次，建模时的模态截断技术会引起控制溢出和观测溢出，导致剩余模态的激励，使控制性能变坏甚至失稳。这些因素使得柔性机械臂的末端精确定位控制不易实现，若仅把关节位置作为系统的输出量进行控制，虽然可以将传递函数转化为最小相位，但显然无法进行振动抑制，必将引起很大的动态跟踪误差；若以系统的名义截断模型进行控制器设计，其鲁棒性又必须得以保证。
发明内容
[0004] 本发明的目的在于提供一种空间柔性机械臂振动抑制算法，通过建立刚柔耦合机械臂系统的动力学模型，并将动力学模型分解为不同尺度慢变和快变两个子系统，将平台的刚性姿态运动状态与挠性振动状态分开，分别设计控制器进行组合控制，有效解决了在轨操控柔性机械臂挠性振动控制问题。
[0005] 为了达到上述目的，本发明通过以下技术方案实现：一种空间柔性机械臂振动抑制算法，其特点是，用于刚柔耦合机械臂系统中，所述的刚柔耦合机械臂系统包含依次连接的中心体、第一柔性机械臂、第二柔性机械臂及一刚性机械臂，该空间柔性机械臂振动抑制算法包含以下步骤：
[0006] S1、在广义坐标系下，建立刚柔耦合机械臂系统的动力学模型；
[0007] S2、对刚柔耦合机械臂系统的动力学模型中的广义变量进行分解，得到快变参数和慢变参数；
[0008] S3、分别针对快变参数和慢变参数建立相应的子系统，并配置对应的控制律；
[0009] S4、将快变子系统的控制律和慢变子系统的控制律进行复合，以定位刚柔耦合机械臂系统的位置并进行振动抑制。
[0010] 所述的快变参数包含柔性机械臂微振动的模态坐标。
[0011] 所述的慢变参数包含中心体的平动位移、中心体的转动姿态角以及机械臂各个关节位移量。
[0012] 所述的步骤S1包含：
[0013] 在中心体质心位置建立本体坐标系Sb，选取广义坐标其中，X为中心体平动变量， 为中心体本体坐标系相对惯性坐标系的姿态角， 为与中心体相连的第一柔性机械臂关节旋转角度， 为第一柔性机械臂与第二柔性臂之间旋转关节的转角， 为刚性机械臂相对于第二柔性机械臂的旋转角度，τ1为第一柔性机械臂的模态坐标，τ2为第二柔性机械臂的模态坐标；
[0014] 设偏速度列阵为 可知
[0015] 利用 关系得到，中心体的各阶 为：
[0016]
[0017] 可得第一柔性机械臂的各阶 为：
[0018]
[0019] 可得第二柔性机械臂的各阶 为：
[0020]
[0021] 可得刚性机械臂的各阶 为：
[0022]
[0023] 得到完整的动力学方程，
[0024] 其中，中心体平动方程形式表示为：
[0025]
[0026] 中心体转动方程形式表示为：
[0027]
[0028] 第一柔性机械臂的转动方程形式表示为：
[0029]
[0030] 第二柔性机械臂的转动方程形式表示为：
[0031]
[0032] 刚性机械臂的转动方程形式表示为：
[0033]
[0034] 第一柔性机械臂的振动方程形式表示为：
[0035]
[0036] 第二柔性机械臂的振动方程形式表示为：
[0037]
[0038] 其中，Jbb为卫星相对整星质心的转动惯量矩阵， 为卫星中心体的姿态角速度，内铰处的连体坐标系已设定为 以此类推；第一柔性机械臂、第二柔性机械臂级一刚性机械臂之间只有一个旋转关节，即有 Π＝[0 0 1]′
表示沿着Z轴旋转。
[0039] 所述的刚柔耦合机械臂系统的动力学模型的标准形式如下：
[0040]
[0041] 式中，θ包含广义坐标中中心体平动和转动六个变量、三个机械臂的旋转关节角度，q包含了二个柔性机械臂的振动坐标，Fθ为与θ变量对应的广义外力，Fq为与变量对应的广义外力，G为各哥氏力，τ为广义模态力，K为刚性矩阵；
[0042] 刚柔耦合机械臂系统的质量矩阵M(θ,q)是一正定矩阵，设它的逆阵为H(θ,q)，那么上式变为：
[0043]
[0044] 上式可分别写为：
[0045]
[0046]
[0047] 定义最小刚度系数为k＝min(kii)，定义μ＝1/k，引入新的变量z＝kq，则有q＝μz，定义 将新的变量代入上式得到：
[0048]
[0049]
[0050] 所述的步骤S2中包含：
[0051] 令μ充分小，则公式 可简化为：
[0052] [M11s(θ,0)]-1＝H11s(θ,0)-H12s(θ,0)[H22s(θ,0)]-1H21s(θ,0)；
[0053] 且
[0054] 式中下标“s”表示向量处于慢变子系统，即变量在慢时标中的计算；
[0055]
[0056]
[0057] 引入快变时标 定义新的状态变量zf1＝z-zs， 式中下标“f”表示变量处于快变子系统，于是式(1)可变换为：
[0058]
[0059] 其中，式(1)表示为：
[0060]
[0061] 由于慢变时标与快变时标相互独立，且在边界层的区域μ→0内慢变分量可视为常数(即 )，此时，如果令μ＝0，得到：
[0062]
[0063] 综合状态变量zf1及上式，可得到以状态方程形式描述的快变子系统为：
[0064]
[0065] 其中，
[0066]
[0067]
[0068]
[0069] τf为快变子系统的控制输入，用于抑制柔性振动，“·”为对快变时标ξ求导；刚柔耦合机械臂系统的总控制输入为τ＝τs+τf；原系统的状态变量近似为θ＝θs+o(μ)，z＝zs+zf1+o(μ)，当μ充分小时，可忽略高阶无穷小项o(μ)。
[0070] 所述的步骤S3中配置慢变子系统的控制律包含：
[0071] 设慢变子系统的动力学如下形式：
[0072]
[0073] 定义跟踪误差为：
[0074] e＝θs-θd
[0075] 定义控制量为：
[0076]
[0077] 其中，K1和K2为正定对角阵，可得动力学模型：
[0078]
[0079] 式中各分量解耦，通过选取K1和K2，可以满足控制需求。
[0080] 所述的步骤S3中配置快变子系统的控制律包含：
[0081] 构造最优控制性能指标函数：
[0082]
[0083] 其中，Q为半正定加权对称矩阵，R为正定加权对称矩阵，于是，快变子系统的最优控制律可设计为：
[0084]
[0085] 并且P为Ricatti方程的解：
[0086]
[0087] 所述的快变子系统采用线性二次型控制。
[0088] 所述的慢变子系统采用非线性PID控制。
[0089] 本发明一种空间柔性机械臂振动抑制算法与现有技术相比具有以下优点：应用奇异摄动理论将系统分解成快慢两个独立的子系统，简化了控制器的设计计算，实现了对两个子系统控制器的独立设计，通过两个控制器的组合得到整个系统的控制输入；采用组合控制方法能够较好地改善系统的稳定控制性能，有效地抑制柔性机械臂的挠性振动，易于工程实现和应用；本发明可使柔性机械臂运动过程微振动对中心体运动和机械臂各关节定位的影响减小，保证中心刚体姿态稳定的基础上，有效地抑制了柔性机械臂的挠性振动，显著改善系统的姿态控制性能。
附图说明
[0090] 图1为本发明一种空间柔性机械臂振动抑制算法的流程图。
具体实施方式
[0091] 以下结合附图，通过详细说明一个较佳的具体实施例，对本发明做进一步阐述。
[0092] 在轨服务柔性机械臂的中心刚体姿态动力学方程中除了有刚性中心体惯性特性效应还有各个柔性机械臂产生的挠性特性影响耦合部分，另外还包含柔性机械臂振动部分对中心刚体作用的累加外力矩部分。
[0093] 如图1所示，一种空间柔性机械臂振动抑制算法，用于刚柔耦合机械臂系统中，所述的刚柔耦合机械臂系统包含依次连接的中心体、第一柔性机械臂、第二柔性机械臂及一刚性机械臂，该空间柔性机械臂振动抑制算法包含以下步骤：
[0094] S1、在广义坐标系下，建立刚柔耦合机械臂系统的动力学模型。
[0095] 在中心体质心位置建立本体坐标系Sb，选取广义坐标其中，X为中心体平动变量， 为中心体本体坐标系相
对惯性坐标系的姿态角， 为与中心体相连的第一柔性机械臂关节旋转角度， 为第一柔性机械臂与第二柔性臂之间旋转关节的转角， 为刚性机械臂相对于第二柔性机械臂的旋转角度，τ1为第一柔性机械臂的模态坐标，τ2为第二柔性机械臂的模态坐标；
[0096] 设偏速度列阵为 可知
[0097] 利用 关系得到，中心体的各阶 为：
[0098]
[0099] 可得第一柔性机械臂的各阶 为：
[0100]
[0101] 可得第二柔性机械臂的各阶 为：
[0102]
[0103] 可得刚性机械臂的各阶 为：
[0104]
[0105] 得到完整的动力学方程，
[0106] 其中，中心体平动方程形式表示为：
[0107]
[0108] 中心体转动方程形式表示为：
[0109]
[0110] 第一柔性机械臂的转动方程形式表示为：
[0111]
[0112] 第二柔性机械臂的转动方程形式表示为：
[0113]
[0114] 刚性机械臂的转动方程形式表示为：
[0115]
[0116] 第一柔性机械臂的振动方程形式表示为：
[0117]
[0118] 第二柔性机械臂的振动方程形式表示为：
[0119]
[0120] 其中，Jbb为卫星相对整星质心的转动惯量矩阵， 为卫星中心体的姿态角速度，内铰处的连体坐标系已设定为 以此类推；第一柔性机械臂、第二柔性机械臂级一刚性机械臂之间只有一个旋转关节，即有 Π＝[0 0 1]′
表示沿着Z轴旋转。
[0121] 刚柔耦合机械臂系统的动力学模型的标准形式如下：
[0122]
[0123] 式中，θ包含广义坐标中中心体平动和转动六个变量、三个机械臂的旋转关节角度，q包含了二个柔性机械臂的振动坐标，Fθ为与θ变量对应的广义外力，Fq为与变量对应的广义外力，G为各哥氏力，τ为广义模态力，K为刚性矩阵；
[0124] 刚柔耦合机械臂系统的质量矩阵M(θ,q)是一正定矩阵，设它的逆阵为H(θ,q)，那么上式变为：
[0125]
[0126] 上式可分别写为：
[0127]
[0128]
[0129] 定义最小刚度系数为k＝min(kii)，定义μ＝1/k，引入新的变量z＝kq，则有q＝μz，定义 将新的变量代入上式得到：
[0130]
[0131]
[0132] S2、对刚柔耦合机械臂系统的动力学模型中的广义变量进行分解，得到快变参数和慢变参数；其中，快变参数包含柔性机械臂微振动的模态坐标，慢变参数包含中心体的平动位移、中心体的转动姿态角以及机械臂各个关节位移量。
[0133] 令μ充分小，则公式 可简化为：
[0134] [M11s(θ,0)]-1＝H11s(θ,0)-H12s(θ,0)[H22s(θ,0)]-1H21s(θ,0)；
[0135] 且
[0136] 式中下标“s”表示向量处于慢变子系统，即变量在慢时标中的计算；
[0137]
[0138]
[0139] 引入快变时标 定义新的状态变量zf1＝z-zs， 式中下标“f”表示变量处于快变子系统，于是式(1)可变换为：
[0140]
[0141] 其中，式(1)表示为：
[0142]
[0143] 由于慢变时标与快变时标相互独立，且在边界层的区域μ→0内慢变分量可视为常数(即 )，此时，如果令μ＝0，得到：
[0144]
[0145] 综合状态变量zf1及上式，可得到以状态方程形式描述的快变子系统为：
[0146]
[0147] 其中，
[0148]
[0149]
[0150]
[0151] τf为快变子系统的控制输入，用于抑制柔性振动，“·”为对快变时标ξ求导；刚柔耦合机械臂系统的总控制输入为τ＝τs+τf；原系统的状态变量近似为θ＝θs+o(μ)，z＝zs+zf1+o(μ)，当μ充分小时，可忽略高阶无穷小项o(μ)。
[0152] S3、分别针对快变参数和慢变参数建立相应的子系统，并配置对应的控制律。
[0153] 配置慢变子系统的控制律包含：
[0154] 设慢变子系统的动力学如下形式：
[0155]
[0156] 由于所考虑的是柔性空间机械臂载体姿态与机械臂各关节铰协调运动的控制，因此系统的控制输出为θs，定义系统的期望输出向量为θd，定义跟踪误差为：
[0157] e＝θs-θd
[0158] 定义控制量为：
[0159]
[0160] 其中，K1和K2为正定对角阵，可得动力学模型：
[0161]
[0162] 式中各分量解耦，通过选取K1和K2，可以满足控制需求。
[0163] 针对快变子系统，由于(Af，Bf)完全可控，因此可采用最优控制理论，即用线性二次型最优控制器(LQR)以实现快变子系统的控制。配置快变子系统的控制律包含：
[0164] 构造最优控制性能指标函数：
[0165]
[0166] 其中，Q为半正定加权对称矩阵，R为正定加权对称矩阵，于是，快变子系统的最优控制律可设计为：
[0167]
[0168] 并且P为Ricatti方程的解：
[0169]
[0170] S4、将快变子系统的控制律和慢变子系统的控制律进行复合，以定位刚柔耦合机械臂系统的位置并进行振动抑制；快变子系统采用线性二次型控制，用于抑制振动，慢变子系统采用非线性PID控制，用于提高刚体运动的鲁棒性。
[0171] 系统总的控制输入则为
[0172] τ＝τs+τf
[0173] 由于所设计的线性二次型控制器采用的是全状态反馈的形式，而振动测量敏感器测得的需要包括卫星本体位姿以外，还需要测量柔性机械臂振动信息。以此实际操作过程中需要采用激光、光纤传感器等敏感器。
[0174] 尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。
