CN111413995A - 双刚体特征点间相对位置跟踪与姿态同步控制方法及系统 - Google Patents

Abstract

本发明提供一种双刚体特征点间相对位置跟踪与姿态同步控制方法及系统，该方法包括：基于牛顿‑欧拉法，分别建立两刚体的位姿动力学方程；根据建立的两刚体各自的位姿动力学方程以及两刚体的特征点间的相对位姿和相对速度，建立考虑模型不确定性的相对位姿动力学方程；根据相对位姿动力学方程，设计不确定参数自适应律和干扰估计律；结合设计的干扰估计律以及未知惯性参数的在线估计，得到基于相对位姿动力学方程的鲁棒无源自适应控制器。本发明能够克服参数不确定性和外部干扰的影响，使得系统状态包括相对位姿和相对位姿速度可以渐近收敛到零，而且在线估计的参数总是有界的。

Description

双刚体特征点间相对位置跟踪与姿态同步控制方法及系统

技术领域

本发明涉及三维空间中自主运动体的控制以及航天器技术领域，特别是指一种双刚体特征点间相对位置跟踪与姿态同步控制方法及系统。

背景技术

刚体运动控制是三维空间中许多自主运动体的本质，一直是控制工程领域的研究热点。刚体一般经历平移和旋转，其中平移的状态空间为线性欧几里得，而旋转的状态空间属于一个特殊的正交群。因此，多年来许多研究都集中在姿态运动控制问题上，其研究成果可以实现在各种条件下均能实现姿态稳定、姿态跟踪和姿态同步系统的全局稳定或几乎全局稳定，但目前在控制器设计中忽略了刚体的平移。没有将刚体的平移和旋转同时考虑到控制器的设计中去。

对于三维空间中刚体姿态稳定与跟踪控制的代表性成果有很多，如基于视觉的反馈控制、离散脉冲控制、反馈线性化控制、基于系统分解的有限时间控制等。然而，这些控制的研究主要考虑两个刚体或两个航天器质心之间的相对位姿运动，在不考虑刚体特征点与质心偏差影响的情况下可以实现姿态跟踪控制。但是在实际应用中，两个刚体的相对位姿控制任务主要是在其特征点之间进行，而不是在其质心之间进行，因此不应忽略两个刚体的体积。例如，对接端口一般位于执行空间交会对接任务的航天器表面，视觉传感器和标记器也安装在自主飞行器表面，用于相对位姿测量。因此，对三维空间中两刚体特征点之间的相对运动进行建模和控制具有重要意义。

发明内容

本发明要解决的技术问题是提供一种双刚体特征点间相对位置跟踪与姿态同步控制方法及系统，以克服参数不确定性和外部干扰影响，使得系统状态包括相对位姿和相对位姿速度可渐近收敛到零，且在线估计的参数总是有界的。

为解决上述技术问题，本发明提供如下技术方案：

一种双刚体特征点间相对位置跟踪与姿态同步控制方法，所述方法包括：

基于牛顿-欧拉法，分别建立两刚体的位姿动力学方程；其中，在所述两刚体中，其中一个为追踪器，另一个为目标器；

根据建立的两刚体各自的位姿动力学方程以及两刚体的特征点间的相对位姿和相对速度，建立考虑模型不确定性的相对位姿动力学方程；

根据所述相对位姿动力学方程，设计不确定参数自适应律和干扰估计律；

结合设计的干扰估计律以及未知惯性参数的在线估计，得到基于所述相对位姿动力学方程的鲁棒无源自适应控制器，从而通过所述鲁棒无源自适应控制器对所述两刚体进行特征点间的相对位置跟踪与姿态同步控制。

进一步地，所述追踪器的位姿动力学方程的表达式为：

其中，p＝[r^T,σ^T]^T；q＝[v^T,ω^T]^T；A＝diag{-S(ω),O₃}；B＝diag{I₃,G(σ)}；

r和v是追踪器的本体固定坐标系的位置和速度相对于惯性固定坐标系在追踪器的本体固定坐标系中的描述；σ和ω是追踪器相对于其质心的姿态和角速度；I₃和O₃是三阶单位矩阵和零矩阵,S(a)表示任意a∈R³的斜对称矩阵；f和τ是追踪器的控制力和控制扭矩；w和δ是干扰力和转矩输入；l是从追踪器的特征点到追踪器的质心的常值位置矢量；m是追踪器的质量；J为追踪器的惯性矩阵；R³表示三维空间实数集。

进一步地，所述目标器的位姿动力学方程的表达式为：

其中，

A_t＝diag{-S(ω_t),O₃}；B_t＝diag{I₃,G(σ_t)}

r_t,v_t,σ_t,ω_t是目标器的本体固定坐标系相对于惯性坐标系在目标器的本体固定坐标系中描述的位置、速度、姿态和角速度；m_t和J_t是目标器的质量和惯性矩阵；l_t是目标器的特征点到其质心的位置矢量；w_t和δ_t为有界干扰力和转矩输入。

进一步地，在所述追踪器的本体固定坐标系中，两个刚体上的特征点之间的相对姿态和相对速度的表达式为：

其中，r_e,σ_e,v_e,ω_e分别是所述追踪器的本体固定坐标系中两刚体特征点间的相对位置，相对姿态，相对速度和相对角速度；其中，

R是从目标器的本体固定坐标系到追踪器的本体固定坐标系的旋转矩阵，

表示为:

进一步地，所述相对位姿动力学方程的表达式为：

其中，s＝q_e+Λp_e，具有正定对角增益矩阵Λ＝diag{Λ₁,Λ₂}和Λ_i∈R^3×3；

A_e＝A；B_e＝diag{I₃ G(σ_e)}；M_e＝M；C_e＝C；

r_e,σ_e,v_e,ω_e分别是追踪器的本体固定坐标系中两刚体特征点间的相对位置，相对姿态，相对速度和相对角速度；h₀＝M₀[Λ(A_ep_e+B_eq_e)-S_e(q-q_e)]-C₀q；

h_Δ＝M_Δ[Λ(A_ep_e+B_eq_e)-S_e(q-q_e)]-C_Δq；S_e＝diag{S(ω_e),S(ω_e)}，R_e＝diag{R,R}，m₀,m_Δ分别表示已知和未知部分质量，J₀,J_Δ分别表示已知和未知部分惯性矩阵。

进一步地，所述不确定参数自适应律的表达式为：

所述干扰估计律的表达式为：

其中，

分别是θ,

的在线估计值，

分别是

的变化率；

Y＝[y₁,O_3×6；y₂,Y_p]；β,γ,δ均表示自适应律的增益；sign(s)＝[sign(s₁),sign(s₂),…sign(s₆)]^T，函数sign(s)表示为s的符号函数；y1＝α₁-S(l)α₂-S(ω)v+S(ω)S(l)ω,y2＝S(l)α₁-S²(l)α₂-S(l)S(ω)v+S(ω)S²(l)ω；Y_p＝L(α₂)-S(ω)L(ω),α＝[α₁ ^T,α₂ ^T]^T＝Λ(A_ep_e+B_eq_e)-S_e(q-q_e),α_i∈R³(i＝1,2)；对任意a＝[a₁，a₂，a₃]^T，回归矩阵L(a)表示为：

进一步地，所述鲁棒无源自适应控制器的表达式为：

其中，可调反馈增益k＞0，K＝K^T＞0。

相应地，为解决上述技术问题，本发明还提供如下技术方案：

一种双刚体特征点间相对位置跟踪与姿态同步控制系统，所述系统包括：

双刚体位姿动力学方程建立模块，用于基于牛顿-欧拉法，分别建立两刚体的位姿动力学方程；其中，在两刚体中，一个为追踪器，另一个为目标器；

相对位姿动力学方程建立模块，用于根据建立的两刚体各自的位姿动力学方程以及两刚体的特征点间的相对位姿和相对速度，建立考虑模型不确定性的相对位姿动力学方程；

不确定参数自适应律和干扰估计律设计模块，用于根据相对位姿动力学方程，设计不确定参数自适应律和干扰估计律；

控制器建立模块，用于结合设计的干扰估计律以及未知惯性参数的在线估计，得到基于所述相对位姿动力学方程的鲁棒无源自适应控制器，从而通过所述控制器对所述两刚体进行特征点间的相对位置跟踪与姿态同步控制。

本发明的上述技术方案的有益效果如下：

本发明对三维空间中两刚体特征点的相对位姿建模和协调控制进行研究，通过建立两刚体特征点间的相对位姿动力学模型，提出了两刚体相对位置跟踪和姿态同步的自适应协调控制方案，利用经典的鲁棒自适应控制技术，设计了六自由度相对位姿控制器，并设计了范数自适应律来补偿外部干扰，使在线估计大大减少了控制器的计算量。在该控制器下，系统状态包括相对位姿和相对位姿速度可以渐近收敛到零，而且在线估计的参数总是有界的。同时利用经典边界层技术解决了控制抖振现象，并对所提出的控制器进行了无源性分析。

附图说明

图1为本发明第一实施例提供的双刚体特征点间相对位置跟踪与姿态同步控制方法的流程示意图；

图2为本发明提供的两刚体相对位姿运动的场景；

图3为本发明提供的控制器的相对位置和速度随时间变化的曲线图；

图4为本发明提供的控制器控制相对姿态和角速度随时间变化的曲线图；

图5为本发明提供的控制器控制力和力矩随时间变化的曲线图；

图6为本发明提供的控制器自适应估计随时间变化的曲线图；

图7为本发明提供的大不确定性下的相对位置和速度随时间变化的曲线图；

图8为本发明提供的大不确定性下的相对姿态和角速度随时间变化的曲线图；

图9为本发明提供的大不确定性下的控制力和力矩随时间变化的曲线图；

图10为本发明提供的大不确定性下的自适应估计随时间变化的曲线图；

图11为本发明提供的相对位姿闭环控制系统框架示意图。

具体实施方式

为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。

第一实施例

请参阅图1至图11，本实施例提供一种双刚体特征点间相对位置跟踪与姿态同步控制方法，如图1所示，该方法包括以下步骤：

S101，基于牛顿-欧拉法，分别建立两刚体的位姿动力学方程；其中，在所述两刚体中，其中一个为追踪器，另一个为目标器；

需要说明的是，在本实施例中，上述步骤具体为：

在图2中定义了三个坐标系和向量；其中，fo＝{Ox_iy_iz_i}是惯性坐标系，ft＝{Px_ty_tz_t}是目标器的固定坐标系，其原点位于特征点P；fc＝{Qxyz}是追踪器的固定坐标系，其原点Q位于追踪器的特征点。C点和T点分别是追踪器和目标器的重心。而且，P和Q也可以位于特征点的延长线上的任何位置。

在追踪器坐标系fc中对位置和姿态运动进行建模，将坐标系fc下的追踪器姿态运动学统一重写为：

其中，p＝[r^T,σ^T]^T；q＝[v^T,ω^T]^T；A＝diag{-S(ω),O₃}，B＝diag{I₃,G(σ)}，r和v是坐标系fc的位置和速度相对于坐标系fo在坐标系fc中的描述；σ和ω是追踪器相对于质心的基于修正的罗德里格斯参数描述的刚体姿态和角速度；I₃和O₃是三维单位矩阵和零矩阵，S(a)表示任意a∈R³的斜对称矩阵，R³表示三维空间实数集。

如图2所示，追踪器质心的位置矢量在坐标系fo表示为：

r_c＝R_c(r+l) (2)

其中，l是从点Q到追踪器质心C的常值位置矢量，R_c是从坐标系fc到坐标系fo的旋转矩阵。

由牛顿第二定律，坐标系fo中表示的位置动力学为：

式中，m是追踪器的质量；f是追踪器的控制力；w代表干扰。

对(2)中的r_c在坐标系fc中求两次时间导数得到：

将(4)代入(3)可得到点Q的位置动力学为：

基于刚体动量矩定理，追踪器相对于质心C的姿态动力学为：

其中，J为追踪器的惯性矩阵；τ为追踪器的控制力矩；δ为追踪器的未知干扰动力矩。

根据理论力学中的平行轴定理，追踪器相对于点Q的惯性矩阵为J_c＝J+m(l^TlI₃-ll^T)，从而可以得到：

J_cω＝Jω+ml×ω×l＝Jω-ml×l×ω (7)

对(7)中ω求时间导数:

再由(5)可以得出：

将(9)代入(8)可得到：

然后，将(7)和(10)代入(6)得到：

将(11)代入(6)可以得到：

将(7)和(8)代入(12)可得到在fc中所表示的追踪器特征点的姿态动力学为：

由(5)和(13)可知在追踪器的本体固定坐标系中表示的位姿运动学方程为：

其中，p＝[r^T,σ^T]^T；q＝[v^T,ω^T]^T；A＝diag{-S(ω),O₃}；B＝diag{I₃,G(σ)}；

r和v是坐标系fc(追踪器的本体固定坐标系)的位置和速度相对于坐标系fo(惯性固定坐标系)在坐标系fc中的描述；σ和ω是追踪器相对于质心的姿态和角速度；I₃和O₃是三阶单位矩阵和零矩阵,S(a)表示任意a∈R³的斜对称矩阵；f和τ是控制力和控制扭矩；w和δ是干扰力和转矩输入。

在目标器坐标系中描述了非受控目标的位姿运动学和动力学：

其中，

A_t＝diag{-S(ω_t),O₃}；B_t＝diag{I₃,G(σ_t)}

r_t,v_t,σ_t,ω_t是坐标系ft(目标器的本体固定坐标系)相对于坐标系fo在ft中描述的位置、速度、姿态和角速度；m_t和J_t是目标的质量和惯性矩阵；l_t是特征点到质心的位置矢量；w_t和δ_t有界干扰力和转矩输入。

S102，根据建立的两刚体各自的位姿动力学方程以及两刚体的特征点间的相对位姿和相对速度，建立考虑模型不确定性的相对位姿动力学方程；

需要说明的是，在本实施例中，上述步骤具体为：

在坐标系fc(追踪器的本体固定坐标系)中两个刚体上的特征点之间的相对姿态和速度可表示为：

其中，r_e,σ_e,v_e,ω_e分别是坐标系fc中两刚体特征点间的相对位置，相对姿态，相对速度，相对角速度，其中

R是从坐标系ft到fc的旋转矩阵，表示为:

根据建立的动力学方程以及两刚体特征点间的相对位姿和相对速度定义式(16)，建立考虑模型不确定性的相对位姿动力学方程。

其中，相对动力学方程表示为：

其中，中间变量s＝q_e+Λp_e，具有正定对角增益矩阵Λ＝diag{Λ₁,Λ₂}和

A_e＝A；B_e＝diag{I₃ G(σ_e)}；M_e＝M；C_e＝C；

r_e,σ_e,v_e,ω_e分别是坐标系fc中两刚体特征点间的相对位置，相对姿态，相对速度，相对角速度；

S_e＝diag{S(ω_e),S(ω_e)}，R_e＝diag{R,R}，m₀,m_Δ分别表示已知和未知部分质量，J₀,J_Δ分别表示已知和未知部分惯性矩阵。不确定项h_Δ＝M_Δ[Λ(A_ep_e+B_eq_e)-S_e(q-q_e)]-C_Δq可参数化为:h_Δ＝Yθ，并且，Y＝[y₁,O_3×6；y₂,Y_p]；

y1＝α₁-S(l)α₂-S(ω)v+S(ω)S(l)ω,y2＝S(l)α₁-S²(l)α₂-S(l)S(ω)v+S(ω)S²(l)ωY_p＝L(α₂)-S(ω)L(ω),α＝[α₁ ^T,α₂ ^T]^T＝Λ(A_ep_e+B_eq_e)-S_e(q-q_e),α_i∈R³(i＝1,2)对任意a＝[a₁，a₂，a₃]^T，回归矩阵L(a)可以表示为：

S103，根据相对位姿动力学方程，设计不确定参数自适应律和干扰估计律；

需要说明的是，在本实施例中，不确定惯性参数自适应律为：

干扰估计律为：

其中，

分别是θ,

的在线估计值，

分别是

的变化率；

β,γ,δ均表示自适应律的增益；k＞0,K＝K^T＞0,β＞0,γ＞0,δ＞0,sign(s)＝[sign(s₁),sign(s₂),…sign(s₆)]^T，函数sign(s)表示为s的符号函数。

S104，结合设计的干扰估计律以及未知惯性参数的在线估计，得到基于所述相对位姿动力学方程的鲁棒无源自适应控制器，从而通过所述鲁棒无源自适应控制器对所述两刚体进行特征点间的相对位置跟踪与姿态同步控制。

需要说明的是，在本实施例中，得到基于模型的鲁棒无源自适应控制器为：

其中，k＞0,K＝K^T＞0,β＞0,γ＞0,δ＞0,sign(s)＝[sign(s₁),sign(s₂),…sign(s₆)]^T,

分别是θ,

的在线估计值。基于假设条件下

和

上述控制器的相对位置和速度随时间变化的曲线图如图3所示；控制相对姿态和角速度随时间变化的曲线图如图4所示；控制力和力矩随时间变化的曲线图如图5所示；自适应估计随时间变化的曲线图如图6所示；

此外，本实施例的大不确定性下的相对位置和速度随时间变化的曲线图如图7所示；大不确定性下的相对姿态和角速度随时间变化的曲线图如图8所示；大不确定性下的控制力和力矩随时间变化的曲线图如图9所示；大不确定性下的自适应估计随时间变化的曲线图如图10所示。

进一步地，本实施例还包括：

S105，对所设计的自适应闭环控制系统进行无源性理论分析，具体如下：

考虑一般非线性系统：

其中，x∈Rⁿ,u∈R^m,y∈R^m,f,g,h是平滑的满足f(0)＝h(0)＝0,g(0)≠0。

定义1：若系统(21)称为无源，即存在连续非负函数(存储函数)V:Rⁿ→R使得对所有u∈R^m,x(0)∈Rⁿ,t≥0都有不等式

定义2：若具有存储函数V的系统(21)称为严格无源，即存在正定函数S:Rⁿ→R使得对所有u∈R^m,t≥0有

且V(0)＝0。

在所提出的自适应闭环控制系统(如图11所示)中，受控系统可视为两个反馈回路和一个自适应回路的反馈连接。自适应回路包括不确定参数自适应律和扰动估计律。

在图11中，闭环系统中有四个模块和三个回路。

具有输入

和输出y＝s的模块H1是无源的，因为

其中

和V₁(0)＝0。

具有输入u＝s和输出

的模块H2严格无源的，因为

其中

和V₂(0)＝0，当

是正定函数时，由于正定矩阵Λ，且

和k＞0。

具有输入u＝s和输出y＝Ks的模块H3是严格无源的，因为

其中S(t)＝s^TKs是正定函数并且V₃(0)≡0。

具有输入u＝s，输出

的模块H4是无源的，因为

而且

和V₄(0)＝0。

引理1：考虑一个严格正则且指数稳定的输入输出系统y＝Hu，如果u是平方可积的，则y是平方可积且有界的；

是平方可积的，并且当t→∞时y→0。

因为H2和H3是严格无源的，H1和H4是无源的，所以可以得出s是平方可积的。然后，从V1+V2+V3+V4＝V(t)，可以得到s，p_e，

是有界信号。此外，由于H2是严格无源的，且在H2中u＝s到y＝p_e的输入输出系统是严格正则且指数稳定的。因此，从引理1出发，有p_e和

平方可积有界；p_e一致连续；当t→∞时p_e(t)→0。因此，从H2和s＝q_e+Λp_e可以得出s(t)＞0和q_e(t)＞0当t＞∞。因此本实施例所提出的自适应控制器是无源控制器。

本实施例对三维空间中两刚体特征点的相对位姿建模和协调控制进行研究，通过建立两刚体特征点间的相对位姿动力学模型，提出了两刚体相对位置跟踪和姿态同步的自适应协调控制方案，利用经典的鲁棒自适应控制技术，设计了六自由度相对位姿控制器，并设计了范数自适应律来补偿外部干扰，使在线估计大大减少了控制器的计算量。在该控制器下，系统状态包括相对位姿和相对位姿速度可以渐近收敛到零，而且在线估计的参数总是有界的。同时利用经典边界层技术解决了控制抖振现象，并对所提出的控制器进行了无源性分析。

第二实施例

本实施例提供一种双刚体特征点间相对位置跟踪与姿态同步控制系统，该双刚体特征点间相对位置跟踪与姿态同步控制系统包括：

双刚体位姿动力学方程建立模块，用于基于牛顿-欧拉法，分别建立两刚体的位姿动力学方程；其中，在两刚体中，一个为追踪器，另一个为目标器；

相对位姿动力学方程建立模块，用于根据建立的两刚体各自的位姿动力学方程以及两刚体的特征点间的相对位姿和相对速度，建立考虑模型不确定性的相对位姿动力学方程；

不确定参数自适应律和干扰估计律设计模块，用于根据相对位姿动力学方程，设计不确定参数自适应律和干扰估计律；

控制器建立模块，用于结合设计的干扰估计律以及未知惯性参数的在线估计，得到基于相对位姿动力学方程的鲁棒无源自适应控制器，从而通过鲁棒无源自适应控制器对所述两刚体进行特征点间的相对位置跟踪与姿态同步控制。

本实施例的双刚体特征点间相对位置跟踪与姿态同步控制系统与上述第一实施例的双刚体特征点间相对位置跟踪与姿态同步控制方法相互对应，其中，本实施例的双刚体特征点间相对位置跟踪与姿态同步控制系统中的各模块单元所实现的功能与上述方法中的各流程步骤一一对应；故，在此不再赘述。

此外，需要说明的是，本领域内的技术人员应明白，本发明实施例的实施例可提供为方法、装置、或计算机程序产品。因此，本发明实施例可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质上实施的计算机程序产品的形式。

本发明实施例是参照根据本发明实施例的方法、终端设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、嵌入式处理机或其他可编程数据处理终端设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理终端设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理终端设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理终端设备上，使得在计算机或其他可编程终端设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程终端设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

还需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。

最后需说明的是，以上所述是本发明的优选实施方式，应当指出，尽管已描述了本发明的优选实施例，但对于本领域普通技术人员来说，一旦得知了本发明的基本创造性概念，在不脱离本发明所述原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明实施例范围的所有变更和修改。

Claims (8)

1.一种双刚体特征点间相对位置跟踪与姿态同步控制方法，其特征在于，所述双刚体特征点间相对位置跟踪与姿态同步控制方法包括：

基于牛顿-欧拉法，分别建立两刚体的位姿动力学方程；其中，在所述两刚体中，其中一个为追踪器，另一个为目标器；

根据建立的两刚体各自的位姿动力学方程以及两刚体的特征点间的相对位姿和相对速度，建立考虑模型不确定性的相对位姿动力学方程；

根据所述相对位姿动力学方程，设计不确定参数自适应律和干扰估计律；

结合设计的干扰估计律以及未知惯性参数的在线估计，得到基于所述相对位姿动力学方程的鲁棒无源自适应控制器，从而通过所述鲁棒无源自适应控制器对所述两刚体进行特征点间的相对位置跟踪与姿态同步控制。

2.如权利要求1所述的双刚体特征点间相对位置跟踪与姿态同步控制方法，其特征在于，所述追踪器的位姿动力学方程的表达式为：

其中，p＝[r^T,σ^T]^T；q＝[v^T,ω^T]^T；A＝diag{-S(ω),O₃}；B＝diag{I₃,G(σ)}；

u＝[f^T,τ^T]^T；d＝[w^T,δ^T]^T；

r和v是追踪器的本体固定坐标系的位置和速度相对于惯性固定坐标系在追踪器的本体固定坐标系中的描述；σ和ω是追踪器相对于其质心的姿态和角速度；I₃和O₃是三阶单位矩阵和零矩阵,S(a)表示任意a∈R³的斜对称矩阵；f和τ是追踪器的控制力和控制扭矩；w和δ是干扰力和转矩输入；l是从追踪器的特征点到追踪器的质心的常值位置矢量；m是追踪器的质量；J为追踪器的惯性矩阵；R³表示三维空间实数集。

3.如权利要求2所述的双刚体特征点间相对位置跟踪与姿态同步控制方法，其特征在于，所述目标器的位姿动力学方程的表达式为：

其中，

A_t＝diag{-S(ω_t),O₃}；B_t＝diag{I₃,G(σ_t)}

r_t,v_t,σ_t,ω_t是目标器的本体固定坐标系相对于惯性坐标系在目标器的本体固定坐标系中描述的位置、速度、姿态和角速度；m_t和J_t是目标器的质量和惯性矩阵；l_t是目标器的特征点到其质心的位置矢量；w_t和δ_t为有界干扰力和转矩输入。

4.如权利要求3所述的双刚体特征点间相对位置跟踪与姿态同步控制方法，其特征在于，在所述追踪器的本体固定坐标系中，两个刚体上的特征点之间的相对姿态和相对速度的表达式为：

其中，r_e,σ_e,v_e,ω_e分别是所述追踪器的本体固定坐标系中两刚体特征点间的相对位置，相对姿态，相对速度和相对角速度；其中，

R是从目标器的本体固定坐标系到追踪器的本体固定坐标系的旋转矩阵，

表示为:

5.如权利要求4所述的双刚体特征点间相对位置跟踪与姿态同步控制方法，其特征在于，所述相对位姿动力学方程的表达式为：

其中，s＝q_e+Λp_e，具有正定对角增益矩阵Λ＝diag{Λ₁,Λ₂}和Λ_i∈R^3×3；

A_e＝A；B_e＝diag{I₃ G(σ_e)}；M_e＝M；C_e＝C；

r_e,σ_e,v_e,ω_e分别是追踪器的本体固定坐标系中两刚体特征点间的相对位置，相对姿态，相对速度和相对角速度；h₀＝M₀[Λ(A_ep_e+B_eq_e)-S_e(q-q_e)]-C₀q；

h_Δ＝M_Δ[Λ(A_ep_e+B_eq_e)-S_e(q-q_e)]-C_Δq；S_e＝diag{S(ω_e),S(ω_e)}，R_e＝diag{R,R}，m₀,m_Δ分别表示已知和未知部分质量，J₀,J_Δ分别表示已知和未知部分惯性矩阵。

6.如权利要求5所述的双刚体特征点间相对位置跟踪与姿态同步控制方法，其特征在于，所述不确定参数自适应律的表达式为：

所述干扰估计律的表达式为：

其中，

分别是θ,

的在线估计值，

分别是

的变化率；

Y＝[y₁,O_3×6；y₂,Y_p]；β,γ,δ均表示自适应律的增益；sign(s)＝[sign(s₁),sign(s₂),…sign(s₆)]^T，函数sign(s)表示为s的符号函数；y1＝α₁-S(l)α₂-S(ω)v+S(ω)S(l)ω,y2＝S(l)α₁-S²(l)α₂-S(l)S(ω)v+S(ω)S²(l)ω；Y_p＝L(α₂)-S(ω)L(ω),α＝[α₁ ^T,α₂ ^T]^T＝Λ(A_ep_e+B_eq_e)-S_e(q-q_e),α_i∈R³(i＝1,2)；对任意a＝[a₁，a₂，a₃]^T，回归矩阵L(a)表示为：

7.如权利要求6所述的双刚体特征点间相对位置跟踪与姿态同步控制方法，其特征在于，所述鲁棒无源自适应控制器的表达式为：

其中，可调反馈增益k＞0，K＝K^T＞0。

8.一种双刚体特征点间相对位置跟踪与姿态同步控制系统，其特征在于，所述双刚体特征点间相对位置跟踪与姿态同步控制系统包括：

双刚体位姿动力学方程建立模块，用于基于牛顿-欧拉法，分别建立两刚体的位姿动力学方程；其中，在两刚体中，一个为追踪器，另一个为目标器；

相对位姿动力学方程建立模块，用于根据建立的两刚体各自的位姿动力学方程以及两刚体的特征点间的相对位姿和相对速度，建立考虑模型不确定性的相对位姿动力学方程；

不确定参数自适应律和干扰估计律设计模块，用于根据相对位姿动力学方程，设计不确定参数自适应律和干扰估计律；

控制器建立模块，用于结合设计的干扰估计律以及未知惯性参数的在线估计，得到基于所述相对位姿动力学方程的鲁棒无源自适应控制器，从而通过所述控制器对所述两刚体进行特征点间的相对位置跟踪与姿态同步控制。

Images