CN117532604B - 一种基于立体视觉的目标位姿及高阶运动信息观测方法 - Google Patents

Abstract

一种基于立体视觉的目标位姿及高阶运动信息观测方法，所述方法包括基于立体视觉获取目标特征点的三维空间坐标信息；根据特征点建立目标的稀疏特征线段模型，并建立观测误差模型；根据高阶微分器求解特征点、稀疏特征线段和观测误差的高阶导数，并建立惩罚函数；设计基于惩罚函数的运动参数观测器，并输出最优解。本发明可解决纯视觉条件下目标位姿及其高阶运动参数的同步观测问题，可同步观测到目标的位姿、速度和加速度信息。本发明方法具有观测维度深、观测精度高和收敛速度快的优点。

Description

一种基于立体视觉的目标位姿及高阶运动信息观测方法

技术领域

本发明属于机器人控制技术领域，具体涉及一种基于立体视觉的目标位姿及高阶运动信息观测方法。

背景技术

目标的位姿及运动信息观测是目标识别领域中的一个重要问题。迄今为止，目标的位姿估计问题得到了广泛研究，而目标的各阶次运动信息(位姿及其速度、加速度)的同步观测仍是一个开放性的问题。需要注意的是，很多操作任务面对的都是动态目标，而现有的位姿估计方法大多是针对静态目标的位姿观测，尚未触及动态目标的动力学本质属性。

在面向动态目标的操作任务中，操作机械臂需要根据目标的实时位姿、速度和加速度信息来预测其运动轨迹并制定控制策略，从而执行操作任务。因此，目标的位姿及其高阶运动信息观测对于动态操作任务来说是一项非常重要的、潜在的通用共性技术，对提升操作机械臂的操作精度和可靠性将起到关键性作用。尽管目前已有一些对运动目标运动估计的研究，但其算法架构大多是以位姿和角速度为观测变量的滤波估计模型，包括卡尔曼滤波、迭代扩展卡尔曼滤波等。这些算法架构并非在严格动力学原理下建立，而是一种基于多传感器(视觉、IMU)融合的滤波估计模型，且其仅观测目标的一阶速度信息。纯视觉条件下目标位姿及其高阶运动信息的同步观测问题尚未得到彻底解决。

发明内容

本发明为克服现有技术不足，提出一种基于立体视觉的目标位姿及高阶运动信息观测方法，该方法可解决纯视觉条件下目标位姿及其高阶运动信息的同步观测问题，可同步观测到目标的位姿、速度和加速度信息。该方法具有观测维度深、观测精度高和收敛速度快的优点。

一种基于立体视觉的目标位姿及高阶运动信息观测方法包括如下：

S1、基于立体视觉获取目标特征点的三维空间坐标信息；

S2、根据特征点建立目标的特征线段模型，并建立观测误差模型；

S3、根据高阶微分器求解特征点、特征线段和观测误差的高阶导数，并建立惩罚函数；

S4、设计基于惩罚函数的运动参数观测器，并输出最优解。

进一步地，所述步骤S1中获取N对特征点在绝对坐标系下的空间坐标，以及N对特征点在目标坐标系下的空间坐标，每对特征点包含2个特征点。

进一步地，步骤S2中2个特征点之间的空间距离为一个特征线段，N个特征线段的集合构成了所述目标的特征线段模型C和T，C表示特征线段模型在绝对坐标系下的描述，T表示特征线段模型在目标坐标系下的描述，C＝{C_i}，C_i为第i个特征线段在绝对坐标系下的描述，T＝{T_i}，i＝1,2，...N，T_i为第i个特征线段在目标坐标系下的描述；建立观测误差模型e_C＝F(Φ)-C，其中F(Φ)＝{F_i(Φ)}，F(Φ)表示根据欧拉角观测值求出的特征线段，F_i(Φ)表示根据欧拉角观测值求出的第i个特征线段，R(Φ)表示旋转矩阵，Φ为待观测目标的欧拉角。

进一步地，步骤S3中所述高阶微分器设计过程为：

S31、根据微分器输出建立误差函数，首先定义是一个与时间t有关的有界且可导的输入函数，微分器的输出为信号的估计值/>和一阶导数估计值/>则误差函数定义为/>

S32、根据误差函数建立误差的长短期计算模型∈为待设计参数；

S33、根据误差的长短期计算模型建立误差积分项h，h满足表达式式中λ，γ为待设计参数；

S34、根据误差的长短期计算模型、误差积分项建立微分器，其表达式为：

其中，n₁、n₂、m₁、m₂为待设计的参数，erf(·)为高斯误差函数，是随时间单调递减的函数，/>ρ，ξ>0为待设计参数；

采用第一个微分器中步骤S34的输出信号作为下一个微分器中步骤S31的输入信号，重复步骤S31-S34，获得信号x的高阶导数，生成高阶微分器。

进一步地，步骤S4中设计的基于惩罚函数的运动参数观测器的状态空间方程为：

式中，Y₁＝Φ，Y₂＝Φ⁽¹⁾，Y₃＝Φ⁽²⁾，分别表示待观测目标的欧拉角，欧拉角变化速度，欧拉角变化加速度；Z表示惩罚函数； C表示特征线段模型，/>为雅可比矩阵，为雅可比矩阵的左伪逆矩阵，/>ι为待设计的参数。

本发明相比现有技术的有益效果是：

一、与现有大多数面向静态目标的位姿估计算法不同，本发明是面向动态目标的位姿及高阶运动信息观测算法，既能用于对运动目标的位姿及高阶运动信息观测，也能用于对静态目标的位姿估计。

二、传统的运动估计方法多是基于多传感器(视觉+IMU)融合的滤波估计模型，且仅观测到目标的速度信息。与传统方法相比，本发明基于立体视觉，即可实现对目标位姿、速度和加速度等高阶运动参数的实时同步观测，具有观测维度更深(加速度级)、收敛速度更快(<1s)、观测精度更高的优点。

三、本发明以目标动力学原理为基础，仅根据目标的特征点空间坐标信息即可实现对目标的位姿及高阶运动信息观测，因此，其应用具有通用性和可拓展性，适用于立体相机、运动捕捉系统等可获取特征点信息的任意测量系统。

附图说明

图1为本发明基于立体视觉的目标位姿及高阶运动信息观测方法的流程图；

图2为本发明中高阶微分器的实现流程图；

图3为实施例1中利用本发明方法求解动态目标位姿及高阶运动信息时所得到的观测结果图；

图4为实施例1中利用本发明方法求解动态目标位姿及高阶运动信息时所得到的观测误差图；

图5为实施例2中利用本发明方法求解静态目标位姿时所得到的观测结果图；

图6为实施例2中利用本发明方法求解静态目标位姿时所得到的观测误差图；

上述图中，x,y,z分别表示绝对坐标系的3个坐标轴。

具体实施方式

下面将结合附图对本发明技术方案的实施例进行详细的描述。除非另有说明，本申请使用的技术术语或者科学术语应当为本发明所属领域技术人员所理解的通常意义。

参见图1所示，一种基于立体视觉的目标位姿及高阶运动信息观测方法包含如下：

S1、基于立体视觉获取目标特征点的三维空间坐标信息；

S2、根据特征点建立目标的特征线段模型，并建立观测误差模型；

S3、根据高阶微分器求解特征点、特征线段和观测误差的高阶导数，并建立惩罚函数；

S4、设计基于惩罚函数的运动参数观测器，并输出最优解。

具体地，所述步骤S1中通过双目相机或RGB-D相机或者能获取目标特征点空间坐标信息的任意测量系统获得N对特征点在绝对坐标系下的空间坐标，以及N对特征点在目标坐标系下的空间坐标，每对特征点包含2个特征点。

进一步地，步骤S2中2个特征点之间的空间距离为一个特征线段，N个特征线段的集合构成了所述目标的特征线段模型C和T，C表示在绝对坐标系下描述的特征线段模型，T表示在目标坐标系下描述的特征线段模型，C＝{C_i}，C_i为第i个特征线段在绝对坐标系下的描述，T＝{T_i}，i＝1,2，...N，T_i为第i个特征线段在目标坐标系下的描述。建立观测误差模型e_C＝F(Φ)-C，其中F(Φ)＝{F_i(Φ)}，F(Φ)表示根据欧拉角观测值求出的特征线段，F_i(Φ)表示根据欧拉角观测值求出的第i个特征线段，R(Φ)表示旋转矩阵，Φ为待观测目标的欧拉角，具体是目标绕绝对坐标系x，y，z三个坐标轴旋转的角度。

通过建立特征线段的方式可以在一定程度上固化目标的几何特征和运动特征，且减小算法的计算量。

进一步地，如图2所示，步骤S3中所述高阶微分器设计过程为：

S31、根据微分器输出建立误差函数，首先定义是一个与时间t有关的有界且可导的输入函数，微分器的输出为信号的估计值/>和一阶导数估计值/>则误差函数定义为/>

S32、根据误差函数建立误差的长短期计算模型∈为待设计参数；综合考虑误差的长期、短期影响，可以有效地提高微分器的估计精度和稳定性；

S33、根据误差的长短期计算模型建立误差积分项h，h满足表达式式中λ，γ为待设计参数；误差积分项h的引入可以有效地增强微分器对噪声的鲁棒性；

S34、根据误差的长短期计算模型、误差积分项建立微分器，其表达式为：

其中，n₁、n₂、m₁、m₂为待设计的参数，erf(·)为高斯误差函数，是随时间单调递减的函数，/>ρ，ξ>0为待设计参数；/>项通过单调递减函数/>可以有效缓解未建模项和误差项所引发的扰动干涉问题。

采用第一个微分器中步骤S34的输出信号作为下一个微分器中步骤S31的输入信号，重复步骤S31-S34，获得信号x的高阶导数，生成高阶微分器。

进一步地，步骤S3中求解的特征点、特征线段和观测误差的高阶导数分别为P^(k)，C^(k)和E^(k)，式中k＝0,1,2,3，其中，P，P⁽¹⁾和P⁽²⁾代表了目标特征点的平移运动信息(位置、速度和加速度)。

具体地，步骤S3中建立的惩罚函数其中，/>为表示观测误差的积分，λ₀，λ₁，λ₂>0，λ₀，λ₁，λ₂是待设计参数，/>综合考虑系统的观测误差、误差累积量和瞬时变化量，本步骤中，惩罚函数Z考虑误差的累积量能有助于提升系统观测精度，考虑误差的瞬时变化量有助于加强观测器运行的稳定性。

基于上述，步骤S4中设计的基于惩罚函数的运动参数观测器的状态空间方程为：

式中，Y₁＝Φ，Y₂＝Φ⁽¹⁾，Y₃＝Φ⁽²⁾，分别表示目标的欧拉角，欧拉角变化速度，欧拉角变化加速度，Z表示惩罚函数；

为雅克比矩阵，/>为雅可比矩阵的左伪逆矩阵。C表示在绝对坐标系下描述的特征线段模型；F(Y₁)表示根据欧拉角观测值求出的特征线段模型，C^(k)表示特征线段的k阶微分，ι为待设计的参数。

在本步骤中，该运动参数观测器可以实时输出收敛速度快、观测精度高的最优解，包含欧拉角Φ、欧拉角变化速度Φ⁽¹⁾和欧拉角变化加速度Φ⁽²⁾。

下面以实施例对本申请内容作进一步阐述：

实施例1，本实施例是利用本发明来观测运动目标的位姿及高阶运动信息：

步骤一：为了模拟出目标在三维空间中的状态，以目标的平移运动轨迹P(t)＝[100sin(0.5t)+20co s(t)；180cos(0.2t)；80sin(0.25t)]^r(mm)，旋转运动轨迹Φ(t)＝[10sin(0.1t)+cos(0.5t)；15sin(0.3t)；12sin(0.05t)]^T(°)为例说明，模拟选定目标上的2对特征点(也就是4个不同线特征点)，则可根据P(t)和Φ(t)计算出2对特征点在绝对坐标系下的空间坐标，以及2对特征点在目标坐标系下的空间坐标。

在实际应用中，本步骤中的特征点空间坐标是根据立体相机或其它能获取目标特征点信息的测量系统获取的(通过立体相机采集到图片，然后基于现有技术特征点识别算法识别出图片中目标上的特征点，获得特征点在相机坐标系下的描述，再基于现有相机标定技术获得相机坐标系与绝对坐标系之间的关系，进而可以获得特征点在绝对坐标系下的描述，上述设定轨迹仅是为了说明本发明的实施例，本步骤只为获取特征点的空间坐标信息；

步骤二：建立目标的特征线段模型其中C_i为第i个特征线段在绝对坐标系下描述，其中T_i为第i个特征线段在目标坐标系下描述。并建立系统的观测误差模型为e_C＝F(Φ)-C，其中/>Φ为待观测的欧拉角(目标绕绝对坐标系x，y，z三个坐标轴旋转的角度)。

步骤三：根据高阶微分器求出特征点、特征线段和观测误差的高阶导数分别为P^(k)，C^(k)和E^(k)，k＝0,1,2,3，其中，其中，P，P⁽¹⁾和P⁽²⁾代表了目标特征点的平移运动信息(位置、速度和加速度)。在此步骤中，选取的微分器参数为：λ＝n₁＝n₂＝1，γ＝0.01，m₁＝m₂＝∈＝60，ρ＝300，ξ＝0.001；

综合考虑系统的观测误差、误差累积量和瞬时变化量，建立了惩罚函数其中，/>本步骤中，选取的参数为λ₀＝4096，λ₁＝768，λ₂＝48；

步骤四：设计基于惩罚函数的运动参数观测器，其状态空间方程如下：

式中，Y₁＝Φ，Y₂＝Φ⁽¹⁾，Y₃＝Φ⁽²⁾， 选取ι＝0.001。

在本步骤中，运动参数观测器可以实时输出收敛速度快、观测精度高的最优解，包含欧拉角Φ、欧拉角变化速度Φ⁽¹⁾和欧拉角变化加速度Φ⁽²⁾。

经过上述步骤，得到动态目标的位姿及高阶运动参数的观测结果和观测误差如图3和图4所示，包含了目标的位置、速度、加速度、欧拉角、欧拉角变化速度和欧拉角变化加速度。从图3和图4中可以看出，本发明可以迅速、高精度的观测到目标真实的位姿及高阶运动信息，包含目标的位姿(位置/姿态)、速度(速度/欧拉角变化速度)和加速度(加速度/欧拉角变化加速度)信息，具有观测维度深(能观测到加速度)、收敛速度快(＜1s)和观测精度高(如图4所示)的优势。且由于本发明仅根据目标特征点的空间坐标即可即可观测到目标的位姿及高阶运动信息，因此其可以仅采用如视觉传感器获取目标的特征点空间坐标，还比如：RGB-D相机、双目相机等。同时本发明也能应用到其他能够定位目标特征点的测量系统中，比如运动捕捉系统。

实施例2，本实施例是利用本发明来观测静态目标的位姿信息。

步骤一：为了模拟出目标在三维空间中的状态，以目标的空间位置P(t)＝[500428 320]^T(mm)和姿态Φ(t)＝[18 -16 5]^T(°)为例。模拟选定目标上的2对特征点(也就是4个不同线特征点)，则可根据P(t)和Φ(t)计算出2对特征点在绝对坐标系下的空间坐标，以及2对特征点在目标坐标系下的空间坐标。

在实际应用中，本步骤中的特征点空间坐标是根据立体相机或其它能获取目标特征点信息的测量系统来实时获取的，上述设定轨迹仅是为了说明本发明的实施例。

步骤二：建立目标的特征线段模型其中C_i为第i个特征线段在绝对坐标系下描述，其中T_i为第i个特征线段在目标坐标系下描述。并建立系统的观测误差模型为e_C＝F(Φ)-C，其中/>Φ为待观测的欧拉角，是指目标绕绝对坐标系x，y，z三个坐标轴旋转的角度。

步骤三：根据高阶微分器求出特征点、特征线段和观测误差的高阶导数分别为P^(k)，C^(k)和E^(k)，k＝0,1,2,3，其中，其中，P，P⁽¹⁾和P⁽²⁾代表了目标特征点的平移运动信息(位置、速度和加速度)。在此步骤中，选取的微分器参数为：λ＝n₁＝n₂＝1，γ＝0.01，m₁＝m₂＝∈＝60，ρ＝300，ξ＝0.001；

综合考虑系统的观测误差、误差累积量和瞬时变化量，建立了惩罚函数其中，/>本步骤中，选取的参数为λ₀＝4096，λ₁＝768，λ₂＝48；

步骤四：设计基于惩罚函数的运动参数观测器，其状态空间方程如下：

式中，Y₁＝Φ，Y₂＝Φ⁽¹⁾，Y₃＝Φ⁽²⁾， 选取ι＝0.001。

在本步骤中，运动参数观测器可以实时输出收敛速度快、观测精度高的最优解，包含欧拉角Φ欧拉角变化速度Φ⁽¹⁾和欧拉角变化加速度Φ⁽²⁾。

经过上述步骤，得到静态目标的位姿观测结果和观测误差如图5和图6所示，包含了目标的位置和欧拉角，其余的速度、加速度信息均收敛至0。从图5和图6中可以看出，本发明可以迅速、高精度的观测到目标真实位姿值，其收敛时间<1s，位置观测误差均为10^-5数量级。

本发明已以较佳实施案例揭示如上，然而并非用以限定本发明，任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内，当可以利用上述揭示的结构及技术内容做出些许的更动或修饰为等同变化的等效实施案例，均仍属本发明技术方案范围。

Claims (6)

1.一种基于立体视觉的目标位姿及高阶运动信息观测方法，其特征在于：包括如下步骤：

S1、基于立体视觉获取目标特征点的三维空间坐标信息；

步骤S1中获取N对特征点在绝对坐标系下的空间坐标，以及N对特征点在目标坐标系下的空间坐标，每对特征点包含2个特征点；

S2、根据特征点建立目标的特征线段模型，并建立观测误差模型；

步骤S2中2个特征点之间的空间距离为一个特征线段，N个特征线段的集合构成了所述目标的特征线段模型C和T，C和T分别表示在绝对坐标系下描述的特征线段模型和在目标坐标系下描述的特征线段模型，C＝{C_i},T＝{T_i}，i＝1,2,…N，其中C_i为第i个特征线段在绝对坐标系下的描述，T_i为第i个特征线段在目标坐标系下的描述，并建立观测误差模型e_C＝F(Φ)-C，其中F(Φ)＝{F_i(Φ)}，F(Φ)表示根据欧拉角观测值求出的特征线段，F_i(Φ)表示根据欧拉角观测值求出的第i个特征线段，R(Φ)表示旋转矩阵，Φ为待观测目标的欧拉角；

S3、根据高阶微分器求解特征点、特征线段和观测误差的高阶导数，并建立惩罚函数；

S4、设计基于惩罚函数的运动参数观测器，并输出最优解。

2.根据权利要求1所述一种基于立体视觉的目标位姿及高阶运动信息观测方法，其特征在于：步骤S3中所述高阶微分器设计过程为：

S31、根据微分器输出建立误差函数，首先定义是一个与时间t有关的有界且可导的输入函数，微分器的输出为信号的估计值/>和一阶导数估计值/>则误差函数定义为

S32、根据误差函数建立误差的长短期计算模型∈为待设计参数；

S33、根据误差的长短期计算模型建立误差积分项h，h满足表达式 式中λ,γ为待设计参数；

S34、根据误差的长短期计算模型、误差积分项建立微分器，其表达式为：

其中，n₁、n₂、m₁、m₂为待设计的参数，erf(·)为高斯误差函数，是随时间单调递减的函数，/> ρ,ξ>0为待设计参数；

采用第一个微分器中步骤S34的输出信号作为下一个微分器中步骤S31的输入信号，重复步骤S31-S34，获得信号x的高阶导数，生成高阶微分器。

3.根据权利要求1所述一种基于立体视觉的目标位姿及高阶运动信息观测方法，其特征在于：

步骤S3中求解的特征点、特征线段和观测误差的高阶导数分别为P^(k),C^(k)和E^(k)，式中k＝0,1,2,3。

4.根据权利要求3所述一种基于立体视觉的目标位姿及高阶运动信息观测方法，其特征在于：步骤S3中建立的惩罚函数其中，/> 表示观测误差的积分，λ₀,λ₁,λ₂>0，/>λ₀,λ₁,λ₂是待设计参数。

5.根据权利要求1或4所述一种基于立体视觉的目标位姿及高阶运动信息观测方法，其特征在于：步骤S4中设计的基于惩罚函数的动力学参数观测器的状态空间方程为：

式中，Y₁＝Φ，Y₂＝Φ⁽¹⁾，Y₃＝Φ⁽²⁾，分别表示目标的欧拉角，欧拉角变化速度，欧拉角变化加速度，Z表示惩罚函数；

为雅克比矩阵，/>为雅可比矩阵的左伪逆矩阵，ι为待设计的参数。

6.根据权利要求1所述一种基于立体视觉的目标位姿及高阶运动信息观测方法，其特征在于：所述步骤S1中所述基于立体视觉获取是通过双目相机或RGB-D相机获取。