一种基于双目视觉的三维目标运动跟踪方法技术

本发明专利技术公开了一种基于双目视觉的三维目标运动跟踪方法。本发明专利技术中，提出一套解决以上两个技术难题的解决方案，最终实现对三维动态目标的精准跟踪；双目相机检测目标，将二维图像上的运动模型扩展到三维空间，对进行非线性运动的目标进行运动状态描述，并在此基础上进行运动跟踪。利用无迹卡尔曼滤波算法对检测的轨迹进行滤波处理，可以降低因误检或者漏检导致的误差，并且相比于每一帧检测结果形成的检测轨迹，无迹卡尔曼滤波估计的轨迹会更加平滑，实现基于双目视觉的三维目标运动跟踪，可以降低因误检或者漏检导致的误差，并且相比于每一帧检测结果形成的检测轨迹，无迹卡尔曼滤波估计的轨迹会更加平滑，更加接近于真实的运动轨迹。动轨迹。动轨迹。

【技术实现步骤摘要】
一种基于双目视觉的三维目标运动跟踪方法


[0001]本专利技术属于移动机器人
，具体为一种基于双目视觉的三维目标运动跟踪方法。

技术介绍

[0002]运动跟踪算法可以分为基于核函数追踪(Kernel based Tracking)和点追踪(Point based Tracking)。常见的基于核函数追踪的方法有均值漂移追踪(Mean Shift Tracking)和支持向量机(Support Vector Machine)。常见的点追踪的方法有：粒子滤波(Particle Filter)、多假设跟踪(Multi Hypothesis Tracking)、卡尔曼滤波系列算法。其中卡尔曼滤波有两个假设条件：
①
系统是线性的；
②
影响测量的噪声属于白噪声。当这两个条件无法满足时卡尔曼滤波估计算法就不再适用了。实际情况下，由于目标运动估计受其他因素影响，运动具有非线性的特点。对进行非线性运动的目标进行运动估计的方法有：扩展卡尔曼滤波Extended Kalman Filter(EKF)，EKF首先将非线性模型通过泰勒展开实现线性化，然后再对非合作目标进行运动跟踪。EKF在进行非线性系统线性化时采用泰勒展开然后舍弃高阶项会引入截断误差，影响估计精度，无迹卡尔曼滤波(Unscented Kalman Filter)则采用无迹变换来代替泰勒展开来实现非线性系统线性化，避免了截断误差，提高了运动估计的精度。
[0003]但是基于视觉图像的三维运动跟踪技术有两个技术难题，第一个难题是：对三维运动目标建立运动模型。第二个难题是实现运动跟踪算法必须要能克服由于光照、图像质量等问题导致的误检甚至漏检的现象，特别是对进行非线性运动运动目标实现运动跟踪。

技术实现思路

[0004]本专利技术的目的在于：为了解决上述提出的问题，提供一种基于双目视觉的三维目标运动跟踪方法。
[0005]本专利技术采用的技术方案如下：一种基于双目视觉的三维目标运动跟踪方法，所述基于双目视觉的三维目标运动跟踪方法包括以下步骤：
[0006]步骤S1：双目相机检测目标；利用左右图像的视差进行三维重建，获取相机坐标系下目标的三维信息；
[0007]步骤S2：建立3D
‑
CTRV模型；输入检测到的三维目标的位置信息，获取其在相机坐标系下的三维位置，计算相对于相机的方位角和俯仰角，得到三维目标的运动状态；
[0008]步骤S3：无迹卡尔曼滤波运动预测；根据当前帧检测到的三维目标状态和前一帧检测到的三维目标状态，对该目标在下一帧的运动状态进行估计；
[0009]步骤S4：无迹卡尔曼滤波观测更新；在获取下一帧的观测值后，根据预测值和预测观测值更新下一帧的目标运动状态，得到经过无迹卡尔曼滤波处理过后的滤波轨迹，实现基于双目视觉的三维目标运动跟踪。
[0010]在一优选的实施方式中，所述步骤S2中，建立3D
‑
CTRV模型包括以下步骤：在双目
相机运动过程中预测动态目标的运动，然后得到运动目标的预测轨迹，首先需要建立观测坐标系；动态目标运动跟踪是在相机坐标系下进行的，以左相机的光心为相机坐标系的原点，相机坐标系的X轴平行于水平面，垂直于移动机器人前进方向，相机坐标系的Y轴平行于水平面，以移动机器人前进方向为正，相机坐标系的Z轴的定义通过右手法则来定义，垂直于水平面向上为正，由于每一帧的检测到的动态目标A的三维坐标是在当前时刻相机坐标系下的，双目相机在运动过程中相机坐标系时刻在变化，动态目标A的三维坐标也会随之变化；本专利技术将第一帧相机坐标系作为观测坐标系，首先需要将每一帧检测到的目标A三维坐标转换到观测坐标系下：将第1帧、第2帧和第n帧检测到的三维目标A都转换到第一帧相机坐标系下，基于检测结果可以得到一条运动轨迹，即图中的曲线轨迹。
[0011]在一优选的实施方式中，所述步骤S2中，选择恒转速恒速模型来描述动态目标的运动状态；
[0012]三维恒转速恒速模型(3D
‑
CTRV)有两点假设，假设一：运动目标具有固定的转弯速率；假设二：动态目标具有恒定的速度大小；；
[0013]假设动态目标A在第k帧的三维坐标是(x
k y
k z
k
)
T
，动态目标A在第k帧的方位角为θ
k
，俯仰角为动态目标A在第k帧的运动状态方程可以表示为：
[0014][0015]其中动态目标A的方位角和俯仰角可以分别通过公式(2)、公式(3)求得：
[0016][0017][0018]另外，定义动态目标俯仰角速度为ω，方位角速度为φ，根据三维恒转速恒速模型的假设一：运动目标具有固定的转弯速率，ω和φ在第k
‑
1帧到第k帧时间间隔内恒定不变，可以分别通过公式(4)、公式(5)来求得：
[0019][0020][0021]根据恒转速恒速模型的假设二：动态目标具有恒定的速度大小；定义动态目标A从第k
‑
1帧到第k帧时间间隔内的速度大小为v，v恒定不变；可以通过公式(6)来获得：
[0022][0023]根据公式(2)到公式(6)，我们可以得到动态目标A在第k帧的速度Vk：
[0024][0025]在一优选的实施方式中，所述步骤S3中，采用无迹卡尔曼滤波来对运动目标进行运动跟踪；无迹卡尔曼滤波算法可以分为三个阶段：运动预测阶段，观测阶段，更新阶段。
[0026]在一优选的实施方式中，所述运动预测阶段在无迹卡尔曼滤波算法的动态预测阶段，首先要定义动态目标A的状态预测方程和观测方程：
[0027][0028]其中，F(X)表示状态转移方程，状态方程由公式(1)定义，观测方程H(X)由公式(10)定义，根据恒转速恒速模型的假设，在连续两帧之间的状态转移方程可以由公式(9)来表示；
[0029]F(X
k
)＝X
k
+V
k
*Δt
ꢀꢀ
(9)
[0030][0031]公式(8)中，Q
k
为第k帧的预测噪声，R
k+1
为第k+1帧的观测噪声，假设Q
k
和R
k+1
是均值为0，方差为Q和R的高斯白噪声，由于本文研究的状态方程包含了动态目标的三维位置、方位角和俯仰角，是一个5*1阶的矩阵，因此在实验中将方差矩阵Q定义为5*5阶的对角阵；观测量为动态目标的三维位置，是一个3*1阶的矩阵，因此将其方差矩阵定义为3*3阶对角阵；
[0032]接下来需要用无迹变换(Unscented Transform)生成sigma点来代替第k帧的状态方程X
k
，状态方程X
k
有5个变量，要生成11个sigma点，将第k帧生成的sigma点集定义为
[0033][0034]这11个sigma点分别被定义为：
[0035][0036本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于双目视觉的三维目标运动跟踪方法，其特征在于：所述基于双目视觉的三维目标运动跟踪方法包括以下步骤：步骤S1：双目相机检测目标；利用左右图像的视差进行三维重建，获取相机坐标系下目标的三维信息；步骤S2：建立3D
‑
CTRV模型；输入检测到的三维目标的位置信息，获取其在相机坐标系下的三维位置，计算相对于相机的方位角和俯仰角，得到三维目标的运动状态；步骤S3：无迹卡尔曼滤波运动预测；根据当前帧检测到的三维目标状态和前一帧检测到的三维目标状态，对该目标在下一帧的运动状态进行估计；步骤S4：无迹卡尔曼滤波观测更新；在获取下一帧的观测值后，根据预测值和预测观测值更新下一帧的目标运动状态，得到经过无迹卡尔曼滤波处理过后的滤波轨迹，实现基于双目视觉的三维目标运动跟踪。2.如权利要求1所述的一种基于双目视觉的三维目标运动跟踪方法，其特征在于：所述步骤S2中，建立3D
‑
CTRV模型包括以下步骤：在双目相机运动过程中预测动态目标的运动，然后得到运动目标的预测轨迹，首先需要建立观测坐标系；动态目标运动跟踪是在相机坐标系下进行的，以左相机的光心为相机坐标系的原点，相机坐标系的X轴平行于水平面，垂直于移动机器人前进方向，相机坐标系的Y轴平行于水平面，以移动机器人前进方向为正，相机坐标系的Z轴的定义通过右手法则来定义，垂直于水平面向上为正，由于每一帧的检测到的动态目标A的三维坐标是在当前时刻相机坐标系下的，双目相机在运动过程中相机坐标系时刻在变化，动态目标A的三维坐标也会随之变化；本发明将第一帧相机坐标系作为观测坐标系，首先需要将每一帧检测到的目标A三维坐标转换到观测坐标系下：将第1帧、第2帧和第n帧检测到的三维目标A都转换到第一帧相机坐标系下，基于检测结果可以得到一条运动轨迹，即图中的曲线轨迹。3.如权利要求1所述的一种基于双目视觉的三维目标运动跟踪方法，其特征在于：所述步骤S2中，选择恒转速恒速模型来描述动态目标的运动状态；三维恒转速恒速模型(3D
‑
CTRV)有两点假设，假设一：运动目标具有固定的转弯速率；假设二：动态目标具有恒定的速度大小；；假设动态目标A在第k帧的三维坐标是(x
k y
k z
k
)
T
，动态目标A在第k帧的方位角为θ
k
，俯仰角为动态目标A在第k帧的运动状态方程可以表示为：其中动态目标A的方位角和俯仰角可以分别通过公式(2)、公式(3)求得：其中动态目标A的方位角和俯仰角可以分别通过公式(2)、公式(3)求得：另外，定义动态目标俯仰角速度为ω，方位角速度为φ，根据三维恒转速恒速模型的假设一：运动目标具有固定的转弯速率，ω和φ在第k
‑
1帧到第k帧时间间隔内恒定不变，可以分别通过公式(4)、公式(5)来求得：
根据恒转速恒速模型的假设二：动态目标具有恒定的速度大小；定义动态目标A从第k
‑
1帧到第k帧时间间隔内的速度大小为v，v恒定不变；可以通过公式(6)来获得：根据公式(2)到公式(6)，我们可以得到动态目标A在第k帧的速度V
k
:4.如权利要求1所述的一种基于双目视觉的三维目标运动跟踪方法，其特征在于：所述步骤S3中，采用无迹卡尔曼滤波来对运动目标进行运动跟踪；无迹卡尔曼滤波算法可以分为三个阶段：运动预测阶段，观测阶段，更新阶段。5.如权利要求4所述的一种基于双目视觉的三维目标运动跟踪方法，其特征在于：所述运动预测阶段在无迹卡尔曼滤波算法的动态预测阶段，首先要定义动态目标A的状态预测方程和观测方程：其中，F(X)表示状态转移方程，状态方程由公式(1)定义，观测方程H(X)由公式(10)定义，根据恒转速恒速模型的假设，在连续两帧之间的状态转移方程可以由公式(9)来表示；F(X
k
)＝X
k
+V
k
*Δt
ꢀꢀꢀꢀꢀ
(9)公式(8)中，Q
k
为第k帧的预测噪声，R
k+1

【专利技术属性】
技术研发人员：周树杰，
申请(专利权)人：上海大学，
类型：发明
国别省市：