一种基于新型势函数的合作目标位姿观测器设计方法技术

本发明专利技术公开了一种基于新型势函数的合作目标位姿观测器设计方法，用于空间合作目标的实时位置和姿态估计。常用的位姿估计方法通常包括静态方法和动态方法，其中动态方法结合了观测信息和系统的运动学或动力学信息，在同等条件下通常能得到优于静态方法的估计结果。作为一种动态估计方法，观测器因其结构简单、应用方便而受到重视。针对目前位姿估计观测器常用势函数种类单一的问题，本发明专利技术在原有势函数的基础上，发明专利技术了基于新型势函数的观测器构造方法，可用于空间机械臂对合作目标操作时的同步相对位姿估计。基于本发明专利技术所设计的新型势函数推导的观测器不仅具有和原有观测器相同的理论稳定特性，在实际应用中也有较好的估计效果。果。果。

【技术实现步骤摘要】
一种基于新型势函数的合作目标位姿观测器设计方法


[0001]本专利技术属于导航制导与控制领域，尤其涉及一种基于新型势函数的合作目标位姿观测器设计方法。

技术介绍

[0002]近年来，随着无人飞行器、自动水下航行器等的发展，位姿(位置和姿态)估计算法成为越来越多学者的研究对象，它不仅能用于飞行器自主导航，还能用于空间机械臂操作合作对象的相对导航。常用的位姿估计方法通常包括静态方法和动态方法，前者仅仅利用系统的观测信息，而后者结合了观测信息和系统的运动学或动力学信息，在同等条件下通常能得到更好的估计结果。目前常用的动态位姿估计方法包括滤波器和观测器两类，其中前者利用噪声的统计特性进行设计，难以从理论上分析稳定性，常见滤波器包括EKF、UKF、PF等；而后者是在确定性框架下设计的，可以建立严格的稳定性。
[0003]随着非线性姿态或位姿观测器的迅速发展，利用势函数的梯度来设计观测器的校正项、进而构造稳定的观测器逐渐引起人们的重视。将基于势函数梯度的校正项引入系统的运动学或动力学方程，可以设计几乎全局渐近稳定的位姿观测器(即除系统被初始化于有限个不稳定临界点外，观测器在流形空间中任一点都是收敛的)，保证了几乎全局稳定性。然而，现有的可用于设计观测器的势函数种类太少，而且基于现有势函数设计的观测器在位姿估计性能上也有很大的提升空间。

技术实现思路

[0004]本专利技术解决的问题是：克服现有方案的不足，提供了一种可行度高的基于新型势函数的合作目标位姿观测器设计方法，可普遍应用于航天器、无人飞行器、自动水下航行器等的位姿估计问题，也可用于空间机械臂操作中对合作目标相对于操作平台的位姿估计问题。
[0005]本专利技术目的通过以下技术方案予以实现：一种基于新型势函数的合作目标位姿观测器设计方法，所述方法包括以下步骤：
[0006]步骤S1：设计能够刻画合作目标相对于操作平台的估计位姿信息和真实位姿信息的误差项，该误差项应具备较为简单的表达形式，并且能够真实有效地反映真实相对位姿信息和观测器提供的估计位姿信息之间的偏差；
[0007]步骤S2：基于步骤S1中提供的误差项设计势函数，能够反映势函数具有的能量属性。该势函数应满足以下要求：是关于误差状态的非负值连续可微函数；对任意误差状态，该势函数恒为非负数；势函数等于零当且仅当误差项为单位值，即估计状态等于真实状态。
[0008]步骤S3：对在步骤S2中设计的势函数求梯度，结合非线性控制理论，在该势函数梯度的基础上进一步发展得到可以引入系统运动学或动力学方程的校正项；
[0009]步骤S4：将在步骤S3中设计的校正项引入系统的运动学或动力学方程，并结合非线性控制理论，确保所设计的观测器是理论上稳定的；如果观测器不稳定，返回步骤S2，重
新设计基于误差函数的势函数。
[0010]本专利技术的优点及有益效果在于：
[0011]本专利技术通过较为简单的数学推导得到形式上复杂程度一致的基于势函数的位姿观测器，所设计的观测器能够取得优于现有观测器的估计效果。此外，通过有选择地调节观测器参数，可以调节观测器在收敛阶段和稳定估计阶段的估计特性，得到更为理想的估计效果。
[0012]本专利技术能够在现有势函数的基础上发展一系列新的势函数，并利用新的势函数设计一系列新的位姿估计观测器，给用户提供了较大的选择空间。
附图说明
[0013]图1是机械臂平台和合作目标示意图。
[0014]图2是观测器姿态估计误差。
[0015]图3是观测器位置估计误差。
[0016]图4是观测器角速度偏置估计误差。
[0017]图5是观测器线速度偏置估计误差。
[0018]图6是本专利技术的流程图。
具体实施方式
[0019]下面将更完整地叙述本专利技术所涉及的基于新型势函数的合作目标位姿观测器设计方法的具体实施过程。虽然本部分提供了本公开的示例性实施过程，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施过程所限制，即，本公开构造新型势函数及观测器的思想可以被进一步应用推广到其他类型新型势函数与观测器的设计中。相反，提供该示例性实施过程是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。需要说明的是，在不冲突的情况下，本专利技术中的示例性实施过程及其中的特征可以相互组合。
[0020]在详细介绍本专利技术具体实施方式之前，需要给出必要的先验知识。在以下说明中，和分别代表实数集、非负实数集以及自然数集。表示n维欧几里得空间，表示嵌入在中的n维球体。对于任意矩阵(方阵)，分别使用右上标
“‑
1”和“T”表示方阵的逆和矩阵(方阵)的转置。对于任意的其欧几里得内乘定义为《A,B》＝tr(A
T
B)。对于任意矢量其欧几里得范数为设矩阵其Frobenius范数为
[0021]记惯性参考系为本体系为本体系为和R∈SO(3)表示本体系相对于惯性参考系的位置和姿态，和表示本体系相对于惯性系的平移速度和旋转角速度。
[0022]对于任意的定义映射(
·
)
×
:满足x
×
y＝x
×
y，其中
×
为矢量叉乘算子。定义映射vex(
·
):为(
·
)
×
的逆映射，满足对于任意
[0023]和有vex(ω
×
)＝ω和(vex(Ω))
×
＝Ω成立。刚体的相对位姿可
以由三维特殊正交群表示，即：
[0024][0025]se(3)表示SE(3)的李代数，由下式给出：
[0026][0027]定义楔积映射(
·
)^:
[0028][0029]SE(3)的切空间可以表示为T
g
SE(3)＝{gX|g∈SE(3),X∈se(3)}。定义映射满足对于任意方阵有成立。定义映射满足对于任意
[0030]有成立。设则：
[0031][0032]设方阵定义映射ψ；
[0033][0034]设则有成立。令I
n
表示n维单位方阵，
[0035]设为SO(3)上的角
‑
轴参数化运算符，满足对于有成立。
[0036]对于可微光滑函数f:表示其梯度为定义SE(3)上的左不变黎曼矩阵<
·
,
·
>:满足：
[0037][0038]定义伴随映射Ad
g
(
·
):满足对于任意g∈SE(3),
[0039]有Ad
g
(X)＝gXg
‑1成立。其在上的矩阵表示为：
[0040][0041]对于任意g1,g2∈SE(3),有及
[0042]表示Ad
g
(
·
)的转置为则有：
[0本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于新型势函数的合作目标位姿观测器设计方法，其特征在于，包括以下步骤：步骤S1：设计能够刻画合作目标相对于操作平台的估计位姿信息和真实位姿信息的误差项，该误差项能够反映真实位姿信息和观测器提供的估计位姿信息之间的偏差；步骤S2：设计势函数，该势函数是关于误差状态的非负值连续可微函数；对任意误差状态，该势函数恒为非负数；势函数等于零当且仅当误差项为单位值，即估计状态等于真实状态；步骤S3：对势函数求梯度，结合非线性控制理论，在该势函数梯度的基础上引入系统运动学或动力学方程的校正项；步骤S4：引入系统的运动学或动力学方程，并结合非线性控制理论，确保观测器是稳定的；如果观测器不稳定，返回步骤S2，重新设计基于误差函数的势函数。2.根据权利要求1所述的一种基于新型势函数的合作目标位姿观测器设计方法，其特征在于：在步骤S1中，设g,b
a
分别代表机械臂平台相对于合作目标的位姿和速度偏置的真实值，和分别为对应的估计值，定义位姿估计误差为速度偏置估计误差为3.根据权利要求1所述的一种基于新型势函数的合作目标位姿观测器设计方法，其特征在于：在步骤S2中，势函数分别如下：分别如下：其中，为待定系数。4.根据权利要求1所述的一种基于新型势函数的合作目标位姿观测器设计方法，其特征在于：在步骤S3中，给出的梯度为：将基于梯度的校正项设计为：梯度的校正项设计为：其中，k
i
,r
i
,b
i
,i＝1,...,n分别为n个自选常值增益参数，n个合作目标坐标系中的参考特征矢量或地标点，以及机械臂平台的敏感器在本体系中对r
i
,i＝...

【专利技术属性】
技术研发人员：钟睿，朱航标，庞传科，贾英宏，张军，王楷，
申请(专利权)人：北京航空航天大学，
类型：发明
国别省市：