一种飞行球体轨迹预测方法及装置制造方法及图纸

本发明专利技术公开了一种飞行球体轨迹预测方法及装置，通过线性化带风阻的球体运动模型，扩展风阻系数自由度，迭代最小二乘拟合方法更新扩展风阻系数，再基于模型，采用卡尔曼滤波对当前帧的球体位置进行位姿估计；建立风阻系数与球体机械能的关系，通过球体位置测量值获得机械能当前帧测量值，从而获取风阻系数当前帧测量值，再通过迭代最小方差的方法对风阻系数迭代收敛，最后对带风阻球体运动模型进行前向离散积分预测飞行球体轨迹。本发明专利技术能够较准确地预测空中飞行球体的轨迹，且根据离散积分的分辨率不同可获得密集或稀疏的未来一段时间的轨迹点，通过相关插值方法可以获得轨迹的高阶状态量，可用于空中物体的拦截任务，机器人球类杂耍任务等。球类杂耍任务等。球类杂耍任务等。

【技术实现步骤摘要】
一种飞行球体轨迹预测方法及装置


[0001]本专利技术属于高速运动物体轨迹预测领域，具体涉及一种飞行球体轨迹预测方法及装置。

技术介绍

[0002]对于高速物体轨迹预测，通常需要在非常短的时间内对该物体进行位置估计、长期预测等处理，以抛球初速度10m/s的乒乓球为例，球在空中的有效飞行时间只有1.2到1.4秒左右，再考虑到整个系统的数据处理和传输延迟，留给预测系统的时间将很短，这就对运动分析和轨迹预测的方法提出了非常高的要求。目前基于深度学习的轨迹预测效果较好，但无法获取球体高阶信息量；基于模型的轨迹预测，根据所需环境不同程度地对飞行球体动力学模型进行定向简化，环境适应性较为单一。

技术实现思路

[0003]为解决现有技术的不足，实现提高高速物体轨迹预测精度和环境适应性的目的，本专利技术采用如下的技术方案：
[0004]一种飞行球体轨迹预测方法，包括如下步骤：
[0005]步骤S1：当前球体位置估计，基于飞行球体位置信息和风阻，构建飞行球体动力学模型；根据飞行球体位置信息，获取飞行球体初速度；基于风阻系数，线性化所述动力学模型；根据风阻系数和基于所述初速度的球体初始状态，构建球体状态与时间的关系，得到球体位置、速度随时间的连续模型；基于所述连续模型中球体位置、速度，构建前后两个时刻球体的状态转移模型；基于所述状态转移模型，对前一时刻到当前时刻的状态转移结果，以及当前时刻的球体位置信息进行估计；更新风阻系数；在考虑风阻的情况下，能够更准确地对动捕设备捕捉的飞行球体位置进行位姿估计；/>[0006]步骤S2：飞行球体轨迹预测，构建球体机械能与风阻系数的微分等式关系，基于估计的所述球体位置信息，得到当前时刻的风阻系数，在线更新风阻系数，并对所述动力学模型进行离散积分，获取飞行球体预测轨迹。
[0007]进一步地，所述步骤S1包括如下步骤：
[0008]步骤S11：获取飞行球体空间坐标信息；
[0009]步骤S12：基于飞行球体空间坐标信息和风阻，构建飞行球体动力学模型；
[0010]步骤S13：获取飞行球体初速度；基于初始前后两个时刻飞行球体位置坐标及时间间隔，计算初速度；
[0011]步骤S14：线性化飞行球体动力学模型；通过增加风阻系数的变化自由度来降低对动力学模型的简化误差，对x,y,z三轴解耦处理，获得扩展风阻系数；
[0012]步骤S15：构建球体状态与时间的函数关系，基于球体的初始状态，连续积分获得球体位置、速度随时间的连续函数模型；
[0013]步骤S16：构建球体的状态转移模型，基于连续函数模型，获得前后两个时刻球体
的状态转移方程；
[0014]步骤S17：采用卡尔曼滤波对当前时刻球体位置与速度进行估计；
[0015]步骤S18：在线优化步骤S14中增加风阻自由度和解耦的扩展风阻系数；
[0016]所述步骤S2包括如下步骤：
[0017]步骤S21：构建风阻系数与飞行球体的机械能的微分等式关系，
[0018]步骤S22：在线迭代更新步骤S21的风阻系数；
[0019]步骤S23：离散积分预测未来一段时间的球体轨迹，每一时刻更新风阻系数后，对飞行球体动力学模型，在均匀时域上进行等时间间隔离散积分，以预测未来一段时间内的离散轨迹。
[0020]进一步地，所述步骤S12的动力学模型为：
[0021][0022]式(a)中，仅考虑风阻对球体的影响，s＝[s
x s
y s
z
]T
表示空间中球体位置，g表示重力加速度，K表示风阻系数，K＝2ρAC
D
/m，其中ρ为空气密度，A为球体横截面面接，C
D
为空气阻力系数，表示球体速度，表示球体加速度。
[0023]所述步骤S14中线性化所述动力学模型为：
[0024][0025]其中，K
x
,K
y
,K
z
表示对x,y,z三轴解耦处理，获得扩展风阻系数。
[0026]进一步地，所述步骤S15的球体状态与时间的函数关系为：
[0027][0028][0029]其中i＝x,y,z三轴，表示单轴上飞行球体的起始状态，g
i
＝(g
x
,g
y
,g
z
)＝(0,0,g)表示重力加速度在三轴上解耦，K表示风阻系数，s表示球体位置，表示速度，表示加速度。
[0030]进一步地，所述步骤S16中的状态转移模型为：
[0031][0032]其中s＝[s
x s
y s
z
]T
表示球体位置，表示球体速度，t表示t时刻，K表示风阻系数，ΔT表示前后两个时刻时间间隔，g
i
表示重力加速度在三轴上解耦。
[0033]进一步地，所述步骤S17中，卡尔曼滤波在t时刻的状态估计过程如下：
[0034]x
cor
＝x
t
+K
a
(Z
‑
Hs
t
)
ꢀꢀꢀꢀ
(g)
[0035]其中H＝[I3×
3 03×3]T
，x
cor
表示位姿估计后的球体状态，K
a
表示卡尔曼增益，Z表示球体位置测量值，s表示球体位置。
[0036]进一步地，所述步骤S18中，由于风阻系数K增加了自由度并解耦至三轴，因此需要在线优化解耦的系数K
x
,K
y
,K
z
，通过构建三轴方向的t时刻函数值与t时刻位姿估计值的误差平方目标函数，采用最小二乘法拟合速度随时间t的连续函数曲线(e)以优化系数K
x
,K
y
,K
z
，目标函数如下：
[0037][0038][0039]其中m表示t到t+n间离散的m时刻，表示m时刻位置估计值，采用Ceres非线性优化库，调用非线性最小二乘求解器求解式(h)，求解得到的最优化系数K
x
,K
y
,K
z
将作为下一帧式(f)的状态转移参数。
[0040]进一步地，所述步骤S21中，飞行球体的机械能由表示，联合所述动力学模型，得到机械能与风阻系数K的关系如下：
[0041][0042]其中，(j)式中的位置坐标s此时为s
cor
，此时为s
cor
、分别是步骤S17的位姿估计后的球体状态中，位置部分与速度部分；通过t
k
时刻的球体位置s[k]和球体速度以及式(j)计算风阻系数K的测量值如下：
[0043][0044][0045]其中g＝[0 0 g]T
表示重力加速度在三轴上解耦，δ
k
表示离散时间间隔，表示t
k<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种飞行球体轨迹预测方法，其特征在于包括如下步骤：步骤S1：当前球体位置估计，基于飞行球体位置信息和风阻，构建飞行球体动力学模型；根据飞行球体位置信息，获取飞行球体初速度；基于风阻系数，线性化所述动力学模型；根据风阻系数和基于所述初速度的球体初始状态，构建球体状态与时间的关系，得到球体位置、速度随时间的连续模型；基于所述连续模型中球体位置、速度，构建前后两个时刻球体的状态转移模型；基于所述状态转移模型，对前一时刻到当前时刻的状态转移结果，以及当前时刻的球体位置信息进行估计；更新风阻系数；步骤S2：飞行球体轨迹预测，构建球体机械能与风阻系数的微分等式关系，基于估计的所述球体位置信息，得到当前时刻的风阻系数，在线更新风阻系数，并对所述动力学模型进行离散积分，获取飞行球体预测轨迹。2.根据权利要求1所述的一种飞行球体轨迹预测方法，其特征在于：所述步骤S1包括如下步骤：步骤S11：获取飞行球体空间坐标信息；步骤S12：基于飞行球体空间坐标信息和风阻，构建飞行球体动力学模型；步骤S13：获取飞行球体初速度；基于初始前后两个时刻飞行球体位置坐标及时间间隔，计算初速度；步骤S14：线性化飞行球体动力学模型；增加风阻系数的变化自由度，对x,y,z三轴解耦处理，获得扩展风阻系数；步骤S15：构建球体状态与时间的函数关系，基于球体的初始状态，连续积分获得球体位置、速度随时间的连续函数模型；步骤S16：构建球体的状态转移模型，基于连续函数模型，获得前后两个时刻球体的状态转移方程；步骤S17：采用卡尔曼滤波对当前时刻球体位置与速度进行估计；步骤S18：在线优化步骤S14中增加风阻自由度和解耦的扩展风阻系数；所述步骤S2包括如下步骤：步骤S21：构建风阻系数与飞行球体的机械能的微分等式关系，步骤S22：在线迭代更新步骤S21的风阻系数；步骤S23：离散积分预测未来一段时间的球体轨迹，每一时刻更新风阻系数后，对飞行球体动力学模型，在均匀时域上进行等时间间隔离散积分，以预测未来一段时间内的离散轨迹。3.根据权利要求1所述的一种飞行球体轨迹预测方法，其特征在于：所述步骤S12的动力学模型为：式(a)中，仅考虑风阻对球体的影响，s＝[s
x s
y s
z
]
T
表示空间中球体位置，g表示重力加速度，K表示风阻系数，K＝2ρAC
D
/m，其中ρ为空气密度，A为球体横截面面接，C
D
为空气阻力系数，表示球体速度，表示球体加速度；所述步骤S14中线性化所述动力学模型为：
其中，K
x
,K
y
,K
z
表示对x,y,z三轴解耦处理，获得扩展风阻系数。4.根据权利要求1所述的一种飞行球体轨迹预测方法，其特征在于：所述步骤S15的球体状态与时间的函数关系为：体状态与时间的函数关系为：其中i＝x,y,z三轴，表示单轴上飞行球体的起始状态，g
i
＝(g
x
,g
y
,g
z
)＝(0,0,g)表示重力加速度在三轴上解耦，K表示风阻系数，s表示球体位置，表示速度，表示加速度。5.根据权利要求1所述的一种飞行球体轨迹预测方法，其特征在于：所述步骤S16中的状态转移模型为：其中s＝[s
x s
y s
...

【专利技术属性】
技术研发人员：王进，涂杰，张海运，陆国栋，
申请(专利权)人：余姚市机器人研究中心，
类型：发明
国别省市：