CN111283683B - 一种机器人视觉特征规划轨迹的伺服跟踪加速收敛方法 - Google Patents

Abstract

本发明公开了一种机器人视觉特征规划轨迹的伺服跟踪加速收敛方法，包括以下步骤：S101，建立视觉特征轨迹规划模型，获得特定规划轨迹；S102，均匀分割参变量，进行分段视觉伺服跟踪；S103，定义跟踪产生的最大位移误差与最大旋转误差；S104，最大位移误差约束下，最小化跟踪收敛总时长。本发明通过定义跟踪误差，设置轨迹插值，进行多参数优化，达到紧密跟踪获得全局收敛的同时，尽可能地缩减跟踪收敛时长，提升视觉伺服系统面对动态目标与环境的自适应能力。

Description

一种机器人视觉特征规划轨迹的伺服跟踪加速收敛方法

技术领域

本发明涉及一种机器人视觉特征规划轨迹的伺服跟踪加速收敛方法，属于机器人传感与伺服控制领域。

背景技术

机器人视觉伺服控制系统利用视觉感知信息建立视觉特征误差与机器人末端相机运动速率间的转换关系，用于指导机器人的运动，逐步减少视觉特征误差收敛于期望特征值的同时，亦收敛于机器人末端相机的期望位姿。

当距离期望位姿的旋转量较大时，作为控制输入的特征向量构成及其相应转换矩阵极有可能引致特征局部收敛问题，也即特征值收敛时，相机位姿尚未收敛。当位移量较大时，可能导致伺服过程中失去重要视觉信息，致使控制中断。基于优化的离线路径规划技术可生成满足多个空间约束的相机运动路径，通过对相应特征规划轨迹的伺服跟踪，达成全局收敛。该规划技术的模型和原理相对简单，在图像噪声与模型误差的影响下，通过对特征规划轨迹的跟踪亦可获得较为平滑和安全的相机运动。全局收敛的跟踪耗时及其运动误差，都将极大程度的受到期望规划值的选择，为不断缩小当前特征值与期望规划值之间误差而生成相机运动速率的作用时长与输出更新次数等因素的影响。在不造成过大运动误差的同时，亟待实现快速跟踪，以满足现代复杂动态工业过程对机器人视觉伺服控制提出的需求。

发明内容

针对现有技术中存在的问题，本发明提供了一种机器人视觉特征规划轨迹的伺服跟踪加速收敛方法，使得视觉伺服系统可自任意位姿尽可能快速地跟随特征规划轨迹收敛到期望特征值与期望相机位姿。

为达到上述目的，本发明的技术方案如下：

一种机器人视觉特征规划轨迹的伺服跟踪加速收敛方法，优化视觉伺服跟踪的相关参数，确保不超过限定跟踪误差的同时，尽可能地缩减跟踪收敛至最终特征期望值的总步进/迭代次数，包括以下步骤：

S101，建立视觉特征轨迹规划模型，获得特定规划轨迹；

S102，均匀分割参变量，进行分段视觉伺服跟踪；

S103，定义跟踪产生的最大位移误差与最大旋转误差；

S104，最大位移误差约束下，最小化跟踪收敛总时长。

优选的，所述步骤S101包括建立机器人末端相机运动路径，并沿路投射目标物获得目标视觉特征的规划轨迹，具体包括如下步骤：

S201，设置一个描述多条路径/轨迹的统一参变量ω∈R，R为实数集；依据相机的起始位姿和期望位姿建立相机运动的笛卡尔路径模型为统一参变量ω的多项式模型{r(ω，M)，d(ω，N)}，其中r(ω，M)∈R³包含了相机绕x、y和z轴的旋转量，d(ω，N)∈R³包含了相机沿x、y和z轴的位移量，M、N为路径模型中的系数变量；

S202，根据目标物的位置信息O_p、模型参数O_m及其所选视觉特征，将目标物沿S201中的相机运动的笛卡尔路径投射到内部参数为K的相机中，获得以ω为参变量的多个视觉特征组合向量轨迹模型：

s(ω，M，N)＝f(K，O_p，O_m，r(ω，M)，d(ω，N)，ω)

S203，优化模型变量M、N，得到可满足工作空间和成像面约束条件的相机规划轨迹的位姿{r_p(ω)，d_p(ω)}及其相应特征规划轨迹s_p(ω)。

优选的，所述步骤S102包括均匀分割上述参变量ω，获得规划轨迹的中间插入值，进行分段视觉伺服跟踪，具体包括如下步骤：

S301，设置分割段数λ_section≥1，λ_section∈N，N为自然数集；取轨迹规划参变量ω以1/λ_section为间隔，从ω₁＝1/λ_section到

均匀取值，获得λ_section组特征向量

其中包括了规划轨迹最末端的期望特征值；

S302，将S301中间插入值得到的特征向量

作为轨迹跟踪的中间期望特征值，利用视觉伺服控制器对这些中间期望特征值进行分段跟踪：

当t＝0时，s(t)为规划轨迹起始端的特征向量值，以缩小实时特征向量值s(t)与中间期望特征值

的误差为目标，通过与特征组合相关的转换矩阵L与伺服跟踪增益λ_ibvs＞0生成相机的运动速率向量T_c，

表示对L的估计值求违逆，并按照单位时间计算相机运动的步进位移和旋转角，指导相机从起始位姿沿规划轨迹逐步趋近中间期望特征值

最大特征误差值

逐步缩小至特征误差收敛阈值e_threshold以下，并循环往复直至最终趋近于最后一个期望特征值；

S303，记跟踪收敛总时长为time，它与伺服控制的步进次数成正比；当Δt＝1s时，将跟踪收敛总时长time视同于视觉伺服控制器的总迭代次数；记录伺服控制每经过单位时间引导相机运动到达的位姿为{r_{ibvs_k}，d_{ibvs_k}}，k＝1，...，time。

优选的，所述步骤S103包括依据跟踪收敛总时长，计算每次步进或每经过单位时间对应的相机运动的笛卡尔路径规划值，并据此定义伺服跟踪误差，具体包括如下步骤：

S401，根据S303中统计的跟踪收敛总时长，将轨迹规划参变量ω，按照1/time等间隔分割，得到ω_k，k＝1，...，time，其中ω₁＝1/time；计算对应的规划相机位姿{r_p(ω_k)，d_p(ω_k)}；

S402，根据规划相机位姿和S303中记录的伺服跟踪经过的相机位姿{r_{ibvs_k}，d_{ibvs_k}}，定义最大伺服跟踪误差为：

其中，d_norm为最大位移误差，r_norm为最大旋转角误差。

优选的，所述步骤S104包括在最大位移误差的上限约束值约束下，分割段数、伺服跟踪增益与特征误差收敛阈值为变量，最小化跟踪整条规划轨迹的步进次数，具体包括如下步骤：

S501，设置伺服跟踪误差约束为d_norm≤d_limit，r_norm≤r_limit；将S301中设置的分割段数λ_section、S302中设置的伺服跟踪增益λ_ibvs、特征误差收敛阈值e_threshold作为统一变量v，代入跟踪收敛总时长time的最小化计算中：

S502，将得到的变量解拆分为

与

代入视觉伺服控制器中，并根据机器人视觉伺服系统实际应用中实时读取的瞬时特征向量值s(t)，指导机器人末端相机的运动。

通过上述技术方案，本发明提供的一种机器人视觉特征规划轨迹的伺服跟踪加速收敛方法，生成可满足多个空间约束条件的机器人末端相机运动，确保运动的全局收敛；通过对规划特征轨迹的紧密跟踪避免过大运动误差的同时，尽可能地缩减伺服跟踪全局收敛的步进次数或总体耗时，提高视觉伺服系统面对动态环境与操作目标的自适应能力，满足现代复杂动态工业过程对机器人操作系统提出的高要求。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。

图1为本发明的优化可影响视觉伺服跟踪时长与笛卡尔跟踪误差的参数流程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

本实施例中，如图1所示，一种机器人视觉特征规划轨迹的伺服跟踪加速收敛方法，包括规划特征轨迹、优化轨迹跟踪相关参数，确保不超过限定跟踪误差的同时，尽可能地缩减跟踪收敛至最终特征期望值的伺服迭代/步进次数，包括以下步骤：

S101，建立视觉特征轨迹规划模型，获得特定规划轨迹；

该步骤包括建立机器人末端相机运动路径，并沿路投射目标物获得目标视觉特征的规划轨迹，具体包括如下步骤：

S201，设置一个描述多条路径/轨迹的统一参变量ω∈R，R为实数集；依据相机的起始位姿{r^o，d^o}和期望位姿{r^*，d^*}建立相机运动的笛卡尔路径{r(ω，M)，d(ω，N)}模型为统一参变量ω的二次多项式模型{r(ω，M)，d(ω，N)}：

M＝[m₁，m₂，m₃]^T、N＝[n₁，n₂，n₃]^T为上述多项式模型中的系数变量，r＝[r₁，r₂，r₃]^T包含相机姿态分别绕x、y和z轴的旋转角，对应旋转矩阵为

[r]_×为向量r构成的反对称矩阵；当r为零向量时，对应旋转矩阵R为单位矩阵；上述多项式模型中，r(ω，M)由r₁(ω，m₁)、r₂(ω，m₂)与r₃(ω，m₃)三个关于ω的多项式组成，M∈R³是多项式里面的系数变量，有以下公式更能清晰的表达多项式的关系：

r₁、r₂、r₃分别表示绕x\y\z的旋转角，如果模型化了，就是把它们的取值变化规律用ω的函数(多项式)来描述，

表示旋转角变化的起始值，也就是相机的起始姿态；

表示期望值，也就是相机的期望姿态。起始值与期望值之间的变化路径参数化了，统一参数为ω，ω＝0表示起始端、ω＝1表示终止端，也即期望端。上述m₁、m₂、m₃是变量，没有定值的，它们共同组成了M，通过改变这些变量值，可以改变旋转角的变化规律，或者说路径。

S202，根据目标物的位置信息O_p、模型参数O_m及其所选视觉特征，从相机位姿转换到视觉特征，需要比如相机内参、目标物模型和相对于相机的位置等参数，多个视觉特征组合，指的是s是个向量，不是一个常量。要能完成笛卡尔空间到视觉二维空间的转变，至少需要六个视觉特征，特征误差通过一个转换矩阵

转化到笛卡尔空间的六个方向的速率：沿x\y\z方向的位移速率和绕x\y\z的旋转角速率。比如，如果定准一个目标为球体，就可以给出具体的视觉特征，将目标物沿S201中的相机运动的笛卡尔路径把球体(O_p包含了以期望相机位姿为参考的球心坐标，O_m包含了球体的半径)投射到内部参数为K的相机中，获得以ω为参变量的多个视觉特征组合向量轨迹模型：

s(ω，M，N)＝f(K，O_p，O_m，r(ω，M)，d(ω，N)，ω)

其中，s(ω，M，N)为多个视觉特征组合成的特征向量；上述轨迹模型中，目标物模型参数O_m信息为已知量；目标位置信息O_p，尤其相对期望相机位姿的目标深度信息，其估计值为已知；相机内部参数矩阵K∈R^3×3是一个实数矩阵，包含相机焦距和成像大小等信息，可通过相机标定获得其估计值。

S203，根据工作空间和成像面对视觉特征的约束条件，该约束条件包括但不限于现有技术中公开的防障碍物碰撞、确保视觉特征始终保持在相机视角范围内、目标始终位于相机前方、全局收敛(相机位姿与视觉特征同时达到期望值)等。通过优化模型变量M、N，得到可满足工作空间和成像面约束条件的相机规划轨迹的位姿{r_p(ω)，d_p(ω)}及其相应特征规划轨迹s_p(ω)。

S102，均匀分割上述参变量，进行分段视觉伺服跟踪；

该步骤包括均匀分割S202中的参变量ω，获得规划轨迹的中间插入值，进行分段视觉伺服跟踪，具体包括如下步骤：

S301，设置分割段数λ_section≥1，λ_section∈N，N为自然数集；取轨迹规划参变量ω以1/λ_section为间隔，从ω₁＝1/λ_section到

均匀取值，获得λ_section组特征向量

其中包括了规划轨迹最末端的期望特征值，当ω₀＝0时，和期望相机姿态下获得的期望特征值，当

时；

S302，将S301中间插入值得到的特征向量

去除初始特征值，得到λ_section组特征值作为分段跟踪的期望特征值，记作

利用视觉伺服控制器对这些中间插值进行跟踪：

当t＝0时，s(t)为特征轨迹起始端的特征向量值s_p(ω＝0)，以缩小实时特征向量值与轨迹分割的第一个期望值

之间的误差

ω₁＝1/λ_section为目标，通过与特征组合相关的转换矩阵L与伺服控制增益λ_ibvs＞0生成相机的运动朝向与速率T_c，

表示对L的估计值求违逆，并按照单位时间Δt＝1s计算相机运动的步进位移和旋转角T_cΔt，指导机器人末端相机从起始位姿沿规划轨迹逐步趋近特征期望值

直到最大特征误差

达到设定阈值e_threshold；更替

为

重复以上伺服跟踪步骤；逐步更替

直至达到最终的期望特征值

S303，跟踪收敛总时长t随着相机位姿的步进而逐步递增t＝t+Δt，它与伺服迭代/步进次数成正比；记总时长为time，记伺服跟踪过程中每经过单位时间相机运动到达的位姿为{r_{ibvs_k}，d_{ibvs_k}}，k＝1，...，time；当Δt＝1s时，总时长time等同于S302中视觉伺服控制器的伺服迭代/步进次数；

S103，定义跟踪产生的最大位移误差与最大旋转误差；

该步骤包括依据跟踪收敛总时长，计算每次步进或每经过单位时间对应的相机运动的笛卡尔路径规划值，并据此定义伺服跟踪误差，具体包括如下步骤：

S401，根据S303中统计的跟踪收敛总时长/步进次数time，将规划轨迹与特征轨迹进行分割，以1/time为间隔等分规划参变量ω，获得time个参变量值，记作ω_k，k＝1，...，time，其中ω₁＝1/time和ω_time＝1；计算这些参变量对应的规划轨迹值{r_p(ω_k)，d_p(ω_k)}，k＝1，...，time；

S402，根据S303中记录的伺服跟踪过程所经过的相机位姿{r_{ibvs_k}，d_{ibvs_k}}，k＝1，...，time和S401中计算得到的规划值，定义最大伺服跟踪误差为：

其中，d_norm为最大位移误差，r_norm为最大旋转角误差；

S104，最大位移误差约束下，最小化跟踪收敛总时长，

该步骤包括在最大位移误差的上限约束值约束下，分割段数、伺服跟踪增益与特征误差收敛阈值为变量，最小化跟踪整条规划轨迹的步进次数，具体包括如下步骤：

S501，设置伺服跟踪误差约束为d_norm≤d_limit，r_norm≤r_limit；将S301中设置的分割段数λ_section、S302中设置的伺服跟踪增益λ_ibvs与特征误差收敛阈值e_threshold统一为一个变量v，代入伺服跟踪时长的最小化计算中：

S502，将S502与S501中得到的变量解拆分为

与

并根据机器人视觉伺服系统实际应用中实时读取的瞬时特征向量值s(t)，代入S302中的视觉伺服控制器中，指导机器人末端相机的运动。

本发明通过定义跟踪误差，设置轨迹插值，进行多参数优化，达到紧密跟踪获得全局收敛的同时，尽可能地缩减跟踪收敛时长，提升视觉伺服系统面对动态目标与环境的自适应能力，通过对特征规划轨迹的插值与分段跟踪，减小轨迹跟踪中可能产生的较大笛卡尔空间误差；在不突破跟踪误差限制的前提下，加速伺服跟踪的进程，使得视觉伺服系统得以快速收敛到期望特征值与期望相机位姿，为视觉伺服轨迹规划技术提供了后续快速跟踪支撑，有利于视觉伺服轨迹规划技术面向动态环境的应用。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (1)

1.一种机器人视觉特征规划轨迹的伺服跟踪加速收敛方法，其特征在于，包括以下步骤：

S101，建立视觉特征轨迹规划模型，获得特定规划轨迹；

包括建立机器人末端相机运动的笛卡尔路径，并沿相机运动的笛卡尔路径投射目标物获得目标视觉特征的规划轨迹，具体包括如下步骤：

S201，设置一个描述多条路径/轨迹的统一参变量ω∈R，R为实数集；依据相机的起始位姿和期望位姿建立相机运动的笛卡尔路径为统一参变量ω的多项式模型{r(ω,M),d(ω,N)}，其中r(ω,M)∈R³包含了相机绕x、y和z轴的旋转量，d(ω,N)∈R³包含了相机沿x、y和z轴的位移量，M、N为相机运动的笛卡尔路径中的系数变量；

S202，根据目标物的位置信息O_p、模型参数O_m及其所选视觉特征，将目标物沿S201中的相机运动的笛卡尔路径投射到内部参数为K的相机中，获得以ω为参变量的多个视觉特征组合向量轨迹模型：

s(ω,M,N)＝f(K,O_p,O_m,r(ω,M),d(ω,N),ω)

S203，优化模型变量M、N，得到可满足工作空间和成像面对视觉特征的约束条件的相机规划轨迹的位姿{r_p(ω),d_p(ω)}及其相应特征规划轨迹s_p(ω)；

S102，均匀分割上述参变量，进行分段视觉伺服跟踪；

包括均匀分割S202中的参变量ω，获得规划轨迹的中间插入值，进行分段视觉伺服跟踪，具体包括如下步骤：

S301，设置分割段数λ_section≥1，λ_section∈N,N为自然数集；取轨迹规划参变量ω以1/λ_section为间隔，从ω₁＝1/λ_section到

均匀取值，获得λ_section组特征向量

其中包括了规划轨迹最末端的期望特征值；

S302，将S301中间插入值得到的特征向量

作为轨迹跟踪的中间期望特征值，利用视觉伺服控制器对这些中间期望特征值进行分段跟踪：

当t＝0时，s(t)为规划轨迹起始端的特征向量值，以缩小实时特征向量值s(t)与中间期望特征值

的误差为目标，通过与特征组合相关的转换矩阵L与伺服跟踪增益λ_ibvs＞0生成相机的运动速率向量T_c，

表示对L的估计值求违逆，并按照单位时间计算相机运动的步进位移量和旋转角，指导相机从起始位姿沿规划轨迹逐步趋近中间期望特征值

最大特征误差值

逐步缩小至特征误差收敛阈值e_threshold以下，并循环往复直至最终趋近于最后一个期望特征值；

S303，记跟踪收敛总时长为time，它与伺服控制的步进次数成正比；当Δt＝1s时，将跟踪收敛总时长time视同于视觉伺服控制器的总迭代次数；记录伺服控制每经过单位时间引导相机运动到达的位姿为{r_{ibvs_k},d_{ibvs_k}}，k＝1,…,time；

S103，定义跟踪产生的最大位移误差与最大旋转误差；

包括依据跟踪收敛总时长，计算每次步进或每经过单位时间对应的相机运动的笛卡尔路径规划值，并据此定义伺服跟踪误差，具体包括如下步骤：

S401，根据S303中统计的跟踪收敛总时长，将轨迹规划参变量ω，按照1/time等间隔分割，得到ω_k，k＝1,…,time，其中ω₁＝1/time；计算对应的规划相机位姿{r_p(ω_k),d_p(ω_k)}；

S402，根据规划相机位姿和S303中记录的伺服跟踪经过的相机位姿{r_{ibvs_k},d_{ibvs_k}}，定义最大伺服跟踪误差的计算方法为：

其中，d_norm为最大位移误差，r_norm为最大旋转角误差；

S104，最大位移误差约束下，最小化跟踪收敛总时长；

包括在最大位移误差的上限约束值约束下，分割段数、伺服跟踪增益与特征误差收敛阈值为变量，最小化跟踪整条规划轨迹的步进次数，具体包括如下步骤：

S501，设置伺服跟踪误差约束为d_norm≤d_limit，r_norm≤r_limit；将S301中设置的分割段数λ_section、S302中设置的伺服跟踪增益λ_ibvs、特征误差收敛阈值e_threshold作为统一变量v，代入跟踪收敛总时长time的最小化计算中：

S502，将得到的变量解拆分为

与

代入视觉伺服控制器中，并根据机器人视觉伺服系统实际应用中实时读取的瞬时特征向量值s(t)，指导机器人末端相机的运动。

Images