【技术实现步骤摘要】
一种空间6自由度位姿变换数据的拟合方法及拟合系统
[0001]本专利技术涉及位姿控制
,特别是涉及一种空间6自由度位姿变换数据的拟合方法、一种空间6自由度位姿变换数据的拟合系统。
技术介绍
[0002]随着现代导航精度的提高,对空间姿态变换的要求也越来越高。空间姿态变换一般是由空间坐标变换矩阵表示,而空间坐标变换矩阵通常是由空间6自由度位姿变换数据表示的。空间6自由度包括在空间坐标系中目标物体分别平行于X轴、Y轴、Z轴的平移以及目标物体分别相对于X轴、Y轴、Z轴的旋转。如何精确求解空间6自由度参数,成为精确控制姿态变换的关键。然而现有的空间6自由度位姿变换数据计算方法适用性不强、精度不高。
技术实现思路
[0003]基于此,有必要针对现有空间6自由度位姿变换数据计算方法适用性不强、精度不高的问题,提供一种空间6自由度位姿变换数据的拟合方法。拟合方法包括如下步骤:
[0004]S1:设置采样周期。采集n组连续的空间位姿变换数据。空间位姿变换数据包括平移数据和角度数据。平移数据用于表征在空间坐标系中当前时刻位置相对于前一时刻位置的位置偏移量。角度数据用于表征在空间坐标系中当前时刻角度相对于前一时刻角度的角度偏移量。
[0005]S2:采用最小二乘的球心拟合方法将平移数据转化为拟合平移数据。最小二乘的球心拟合方法包括如下过程:
[0006]S21:将每个平移数据对应表达为空间坐标系中的离散点。
[0007]S22:根据离散点构建外接球体。
[0008]S23:将 ...
【技术保护点】
【技术特征摘要】
1.一种空间6自由度位姿变换数据的拟合方法,其用于表征目标物体的空间位姿变换规律;其中,6自由度位姿变换数据包括在空间坐标系中目标物体分别平行于X轴、Y轴和Z轴的位移量以及目标物体分别相对于X轴、Y轴和Z轴的旋转角度;其特征在于,所述拟合方法包括如下步骤:S1:设置采样周期;采集n组连续的空间位姿变换数据;所述空间位姿变换数据包括平移数据和角度数据;所述平移数据用于表征在空间坐标系中当前时刻位置相对于前一时刻位置的位置偏移量;所述角度数据用于表征在空间坐标系中当前时刻角度相对于前一时刻角度的角度偏移量;S2:采用最小二乘的球心拟合方法将所述平移数据转化为拟合平移数据;最小二乘的球心拟合方法包括如下过程:S21:将每个平移数据对应表达为空间坐标系中的离散点;S22:根据所述离散点构建外接球体;S23:将所述外接球体的球心坐标作为拟合平移数据;S3:采用基向量拟合方法将所述角度数据转化为拟合角度数据;基向量拟合方法包括如下过程:S31:将每个角度数据转化为相应的空间旋转矩阵;将每个空间旋转矩阵的三个列向量作为一组基向量;S32:预设拟合角度数据;将预设的拟合角度数据转化为相应的拟合空间旋转矩阵;将所述拟合空间旋转矩阵的三个列向量作为预设拟合基向量;S33:根据每组基向量与所述预设拟合基向量的夹角建立目标函数;S34:将所述夹角的余弦值与麦克劳林公式代入目标函数;采用多元函数求极值方法在目标函数中求出拟合基向量;S35:根据拟合基向量与空间旋转矩阵求出拟合角度数据;S4:所述拟合平移数据与所述拟合角度数据共同构成拟合空间位姿变换数据;所述拟合空间位姿变换数据用于表征下一时刻的空间位姿相对于当前时刻的空间位姿的变化量。2.根据权利要求1所述的空间6自由度位姿变换数据的拟合方法,其特征在于,在S23中,根据最小二乘法,设定拟合平移数据的拟合目标函数为:对拟合目标函数求偏导并令偏导数为零,则拟合平移数据为:其中,O0(x0,y0,z0)为拟合平移数据,x0为目标物体平行于X轴的拟合位移偏移量,y0为目标物体平行于Y轴的拟合位移偏移量,z0为目标物体平行于Z轴的拟合位移偏移量,O
i
(x
i
,y
i
,z
i
),i=1,2...n为平移数据,x
i
为目标物体在第i个时间周期内平行于X轴的位移偏移量,y
i
为目标物体在第i个时间周期内平行于Y轴的位移偏移量,z
i
为目标物体在第i个时间周期内平行于Z轴的位移偏移量,|O
i
O0|表示点O
i
和点O0之间的距离。
3.根据权利要求1所述的空间6自由度位姿变换数据的拟合方法,其特征在于,在S31中,根据所述角度数据V
i
(α
i
,β
i
,γ
i
)获取的所述空间旋转矩阵R
i
为:其中,α
i
为目标物体在第i个时间周期内相对于X轴的旋转角度,β
i
为目标物体在第i个时间周期内相对于Y轴的旋转角度,γ
i
为目标物体在第i个时间周期内相对于Z轴的旋转角度;则基向量为:为:为:其中,e
ix
为目标物体在第i个时间周期内初始X轴基向量e
x
经过旋转后的基向量,e
ix1
为基向量e
ix
在X轴的分量,e
ix2
为基向量e
ix
在Y轴的分量,e
ix3
为基向量e
ix
在Z轴的分量;e
iy
为目标物体在第i个时间周期内初始Y轴基向量e
y
经过旋转后的基向量,e
iy1
为基向量e
iy
在X轴的分量,e
iy2
为基向量e
iy
在Y轴的分量,e
iy3
为基向量e
iy
在Z轴的分量;e
iz
为目标物体在第i个时间周期内初始Z轴基向量e
z
经过旋转后的基向量,e
iz1
为基向量e
iz
在X轴的分量,e
iz2
为基向量e
iz
在Y轴的分量...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。