CN103878770A - 基于速度估计的空间机器人视觉时延误差补偿方法 - Google Patents

Abstract

基于速度估计的空间机器人视觉时延误差补偿方法，解决了漂浮基座下空间机器人视觉测量时延误差的补偿问题。包括确定系统的视觉时延，建立带时延的视觉测量数据与物理真实相对位姿间的数学关系；根据带时延的视觉测量数据和机械臂的关节指令，估计当前的空间机器人末端速度；设计误差控制器，减小空间机器人末端速度的估计误差；根据校正后空间机器人末端速度估值，对当前带有时延的视觉测量数据进行补偿，得到经过补偿的视觉测量数据。本发明利用历史测量数据融合关节角速度指令估计当前空间机器人末端速度，设计估计误差控制器以减小速度估计的误差；实现漂浮基座下空间机器人精确的视觉时延补偿，有利于实现空间机器人高精度地完成精细操作任务。

Description

基于速度估计的空间机器人视觉时延误差补偿方法

技术领域

本发明涉及一种空间机器人视觉测量时延误差补偿方法。

背景技术

漂浮基座下的空间机器人（包括载体航天器和机械臂）执行空间任务时，面临的一个实际问题是其机械臂上的视觉测量系统，进行视觉信息处理时往往需要消耗较长的时间，其中一个原因是由于光照条件复杂，空间机器人所采用的视觉信息处理方法较复杂，另一个原因是能够用于空间机器人视觉信息处理的处理器计算能力往往很有限，由于视觉信息方法复杂程度和视觉信息处理设备计算能力之间的矛盾导致了视觉测量信息往往具有较大的时延，表现在视觉测量精度上就表现为具有较大的测量误差。由于视觉测量误差的存在，使空间机器人在执行目标捕获等任务时，会影响其操作精度。为了使空间机器人能够更快速、平稳的完成精细的在轨服务任务，在空间机器人系统设计过程中需要考虑空间机器人视觉时延误差的补偿问题。

目前已经有一些可用于地面固定基座机器人视觉时延误差处理的预测和估计方法。最小二乘估计是以误差的平方和最小为准则，根据观测数据估计线性模型中未知参数的一种基本参数估计方法。它的基本思路是选择估计量使模型输出与实测输出之差的平方和达到最小。这种求误差平方和的方式可以避免正负误差相抵，而且便于数学处理。卡尔曼滤波是一种线性最小方差估计，可以估计信号的过去和当前状态，甚至能估计将来的状态。卡尔曼滤波预测模型从与被提取信号有关的测量值中通过方法估计出所需信号。其中被估计信号是由白噪声激励引起的随机响应，激励源与响应之间的系统方程己知，测量量与被估计量之间的函数关系也己知。Smith预估器经常应用于地面机器人视觉时延处理。Smith预估器的作用原理是通过引入时延矫正环节，对带有测量时延的视觉数据进行补偿，从而将系统中的纯时延部分分离到整个闭环系统之外，从而提高整个控制系统的控制性能。从Smith预估器的作用原理可以看出，Smith预估器设计的基础在于必须保证能够准确的预测出被控对象在给定输入状态下的动态特性，才能够补偿掉测量信息中的时延部分，因此Smith预估器非常依赖于建立精确的数学模型。漂浮基座下的空间机器人与地面机器人不同，由于空间机器人没有固定的基座，机械臂的运动与载体航天器的运动耦合在一起，任何空间机械臂的运动都会改变载体航天器的位置和姿态，而载体航天器位置姿态的改变也会反过来影响空间机械臂的定位，因此空间机器人的动力学计算非常复杂。不仅如此，随着载体航天器燃料的不断消耗，载体航天器的质量、质心位置和惯量矩阵都不断在变化，精确的数学模型很难获得，因此经典的预测和估计方法很难在空间机器人应用中获得很好的效果。

发明内容

本发明提供一种基于速度估计的漂浮基座下空间机器人视觉时延误差补偿方法，以减小空间机器人视觉系统因时延存在而产生的视觉测量误差，有利于空间机器人基于补偿后视觉信息完成精细的空间任务。

基于速度估计的空间机器人视觉时延误差补偿方法由以下步骤完成：

步骤一、根据所采用的视觉处理算法和所应用的硬件确定视觉系统的视觉时延，建立带时延的视觉测量数据与物理真实相对位姿（测量相机与目标）间的数学关系；步骤二、根据带有时延的视觉测量数据和机械臂的关节指令，估计当前空间机器人的末端速度(见式(26))；步骤三、设计误差控制器，减小空间机器人末端速度估值的误差，得到校正后空间机器人末端速度估值(见式(30))；步骤四、根据校正后空间机器人末端速度估值，对当前带有时延的视觉测量数据进行补偿，得到经过补偿的视觉测量数据(见式(32))。

本发明具有以下有益效果：本发明方法通过空间机器人数学模型分析，给出了利用历史测量数据融合关节角速度指令估计当前空间机器人末端速度。本发明利用空间机器人视觉系统获得的带时延的测量数据和机械臂关节角速度指令，估计当前空间机器人末端速度，同时设计估计误差控制器，减小空间机器人末端速度估值的误差，根据得到校正后空间机器人末端速度估值，可有效地补偿由于时延引起的漂浮基座下空间机器人对目标的视觉测量误差，可保证空间机器人在轨高精度地完成精细操作任务。

本发明不需建立空间机器人精确的数学模型，利用数学模型分析得到的机械臂真实末端速度与带时延末端速度的简化关系，进行末端速度估计，并采用闭环控制的方法提高了空间机器人视觉时延误差补偿的精度；该方法避免了复杂的动力学计算，计算简单，便于实现漂浮基座下空间机器人精确的视觉时延补偿，有利于实现空间机器人高精度地完成精细操作任务，可用于空间机器人在轨维护、空间碎片清理、深空探测等空间应用领域。

附图说明

图1为本发明的视觉时延误差控制系统框图；图2为本发明的流程图；图3为本发明空间机器人系统和各坐标系示意图；图4为补偿后视觉测量的估计数据与原始测量数据和真实相对位置对比图，其中：图4a为补偿后视觉测量的估计数据与原始测量数据和真实相对位置在x轴方向的对比图，图4b为补偿后视觉测量的估计数据与原始测量数据和真实相对位置在y轴方向的对比图，图4c为为补偿后视觉测量的估计数据与原始测量数据和真实相对位置在z轴方向的对比图；

图5为补偿后视觉测量数据估计误差与未补偿前视觉测量数据时延误差的对比图，其中：图5a为补偿后视觉测量数据估计误差与未补偿前视觉测量数据时延误差在x轴方向的对比图，图5b为视觉测量数据估计误差与未补偿前视觉测量数据时延误差在y轴方向的对比图，图5c为视觉测量数据估计误差与未补偿前视觉测量数据时延误差在z轴方向的对比图。

具体实施方式

具体实施方式一：结合图1、图2和图3说明本实施方式，本实施方式由以下步骤完成：一、根据所采用的视觉处理算法和所应用的硬件确定视觉系统的时延，建立带时延的视觉测量数据与物理真实相对位姿（测量相机与目标）间的数学关系；二、根据带有时延的视觉测量数据和机械臂的关节指令，估计当前空间机器人的末端速度；三、设计误差控制器，减小空间机器人末端速度估值的误差，得到校正后空间机器人末端速度估值；四、根据校正后空间机器人末端速度估值，对当前带有时延的视觉测量数据进行补偿，得到经过补偿的视觉测量数据。

具体实施方式二：本实施方式与具体实施方式一的不同点是：本实施方式在步骤一所述的根据所采用的视觉处理算法和所应用的硬件确定视觉系统的时延，建立带时延的视觉测量数据与物理真实相对位姿（测量相机与目标）间的数学关系为：根据所采用的视觉信息处理方法和所应用的硬件平台确定整个视觉测量环节引起的时延为m个周期，若系统每个周期的时长为t_s，则空间机器人系统的视觉时延T_d为：

T_d=m×t_s        (1)

定义空间机器人k时刻的视觉测量信息D_v(k)和真实距离信息D_r(k)为

D_v(k)=[x_v(k)y_v(k)z_v(k)α_v(k)β_v(k)γ_v(k)]^T      (2)

D_r(k)=[x_r(k)y_r(k)z_r(k)α_r(k)β_r(k)γ_r(k)]^T      (3)

其中k为通用时刻，x(k)，y(k)，z(k)为相对位置信息，α(k)，β(k)，γ(k)为描述相对位姿的欧拉角，空间机器人视觉测量信息D_v(k)和真实距离信息D_r(k)间的关系为

D_r(k)=D_v(k+m)       (4)

k表示任意时刻，m表示时延。

若当前时刻为N，视觉测量信息为D_v(N)，则可知道D_r(N-m)之前的真实距离信息都可由过去时刻的视觉测量数据直接得到。定义真实速度信息V_r(k)为

$V_r\left(k\right)={\left[\begin{array}{cccccc}{\stackrel{\·}{x}}_r\left(k\right)& {\stackrel{\·}{y}}_r\left(k\right)& {\stackrel{\·}{z}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\α}}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\β}}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\γ}}}_r\left(k\right)\end{array}\right]}^T---\left(5\right)$

可以由D_r(k+1)和D_r(k)计算得到

$V_r\left(k\right)=\frac{D_r(k+1)-D_r\left(k\right)}{t_s}---\left(6\right)$

因此空间机器人的真实速度信息V_r(N-m-1)和V_r(N-m-1)之前的真实速度信息也可以通过计算得到，则此时真实的距离信息D_k(N)为

$D_r\left(N\right)=D_v\left(N\right)+t_s\×\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{V}_r(N-i)---\left(7\right)$

具体实施方式三：本实施方式与具体实施方式一或二的不同点是：本实施方式在步骤二所述的根据带有时延的视觉测量数据和机械臂的关节指令，估计当前空间机器人的末端速度,其过程为：

利用带有时延的视觉测量数据，可得到真实速度序列

的估值序列

则可通过速度估值序列对当前的视觉测量信息Dv(N)进行补偿，式(7)可改写为：

${\tilde{D}}_r\left(N\right)=D_v\left(N\right)+t_s\×\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{\tilde{V}}_r(N-i)---\left(8\right)$

从而得到当前的真实距离信息的估计值；

下面将带有时延的空间机器人视觉测量数据与机械臂的关节指令相结合，构造出当前时刻的空间机器人末端速度的估值。

由于处于零重力状态下的空间机器人满足动量守恒和角动量守恒，可依据此建立空间机器人关节运动状态与末端效应器广义速度间的数学关系，满足方程：

$\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]=J_g({\mathrm{\Ψ}}_0,\mathrm{\Θ},m_i,I_i)\stackrel{\·}{\mathrm{\Θ}}=\left[\begin{array}{c}{J}_{g\_v}\\ J_{g\_\mathrm{\ω}}\end{array}\right]\stackrel{\·}{\mathrm{\Θ}}---\left(9\right)$

其中参数矩阵J_g就是空间机器人的广义雅克比矩阵，是惯量矩阵I_i、质量参数m_i和机器人关节角Θ、载体航天器姿态Ψ₀的函数，

是机器人的关节角速度。其中

$v_e={\left[\begin{array}{ccc}{\stackrel{\·}{x}}_r& {\stackrel{\·}{y}}_r& {\stackrel{\·}{z}}_r\end{array}\right]}^T---\left(10\right)$

${\mathrm{\ω}}_e=\left[\begin{array}{ccc}0& -\sin {\mathrm{\α}}_r& \cos {\mathrm{\α}}_r\cos {\mathrm{\β}}_r\\ 0& \cos {\mathrm{\α}}_r& \sin {\mathrm{\α}}_r\cos {\mathrm{\β}}_r\\ 1& 0& -\sin {\mathrm{\β}}_r\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\α}}}_r\\ {\stackrel{\·}{\mathrm{\β}}}_r\\ {\stackrel{\·}{\mathrm{\γ}}}_r\end{array}\right]---\left(11\right)$

v_e和ω_e分别是空间机器人末端执行器的线速度和角速度，

和

为空间机器人末端姿态欧拉角的微分。

将广义雅可比矩阵进行展开，考虑空间机器人的末端广义速度

$\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{J}_{\mathrm{gi}}{\mathrm{\θ}}_i---\left(12\right)$

其中J_gi是广义雅克比矩阵的第i列。

因空间机器人各运动学、动力学参数的真实值都包含在带时延的测量数据中，定义M时刻在当前时刻N的邻域内，将J_gi(M)展开为：

$J_{\mathrm{gi}}\left(M\right)=J_{\mathrm{gi}}\left(N\right)+\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\underset{j=1}{\overset{\∞}{\mathrm{\Σ}}}\frac{1}{j!}\frac{{\∂}^j{J}_{\mathrm{gi}}}{{\∂\mathrm{\θ}}_i^j}{[{\mathrm{\θ}}_i\left(M\right)-{\mathrm{\θ}}_i\left(N\right)]}^j+E_{\mathrm{Ji}}^{\′}\left(M\right)---\left(13\right)$

其中E'_Ji(M)为因载体姿态变化引起的误差。仅保留其中的一次项，进一步变形为

$\left\{\begin{array}{c}{J}_{\mathrm{gi}}\left(M\right)=J_{\mathrm{gi}}\left(N\right)+\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\frac{\∂J_{\mathrm{gi}}\left(N\right)}{{\∂\mathrm{\θ}}_i}[{\mathrm{\θ}}_i\left(M\right)-{\mathrm{\θ}}_i\left(N\right)]+E_{\mathrm{Ji}}\left(M\right)\\ E_{\mathrm{Ji}}\left(M\right)=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\underset{j=2}{\overset{\∞}{\mathrm{\Σ}}}\frac{1}{j!}\frac{{\∂}^j{J}_{\mathrm{gi}}\left(N\right)}{{\∂\mathrm{\θ}}_i^j}{[{\mathrm{\θ}}_i\left(M\right)-{\mathrm{\θ}}_i\left(N\right)]}^j+E_{\mathrm{Ji}}^{\′}\left(M\right)\end{array}\right.---\left(14\right)$

此时M时刻的空间机器人末端速度可以表示为

$\begin{array}{c}\left[\begin{array}{c}{v}_e\left(M\right)\\ {\mathrm{\ω}}_e\left(M\right)\end{array}\right]=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{J}_{\mathrm{gi}}\left(M\right){\stackrel{\·}{\mathrm{\θ}}}_i\left(M\right)\\ =J_g\left(N\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)+{\mathrm{\ΔJ}}_N\left(M\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)+E_J\left(M\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)\end{array}---\left(15\right)$

其中

${\mathrm{\ΔJ}}_N\left(M\right)=\left[\frac{\∂J_{g1}\left(N\right)}{{\∂\mathrm{\θ}}_1}\right[{\mathrm{\θ}}_1\left(M\right)-{\mathrm{\θ}}_1\left(N\right)]\·\·\·\frac{{\∂J}_{\mathrm{gn}}\left(N\right)}{{\∂\mathrm{\θ}}_n}[{\mathrm{\θ}}_n\left(M\right)-{\mathrm{\θ}}_n\left(N\right)\left]\right]---\left(16\right)$

M时刻的空间机器人关节角速度表述为

$\stackrel{\·}{\mathrm{\θ}}\left(M\right)=\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_{\mathrm{\θ}}\left(M\right)---\left(17\right)$

其中

$E_{\mathrm{\θ}}\left(M\right)=\stackrel{\·}{\mathrm{\θ}}\left(M\right)-\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)---\left(18\right)$

由于空间机器人运动时，关节角速度变化比较慢，所以公式错误！未找到引用源。第一项为M时刻空间机器人关节角速度的主要部分，E_θ(M)为与加速度相关的余项；

将式（17）带入式（15），得M时刻的空间机器人末端速度表示为：

$\begin{array}{c}\left[\begin{array}{c}{v}_e\left(M\right)\\ {\mathrm{\ω}}_e\left(M\right)\end{array}\right]=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{J}_{\mathrm{gi}}\left(M\right){\stackrel{\·}{\mathrm{\θ}}}_i\left(M\right)\\ =\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}{J}_g\left(N\right)\stackrel{\·}{\mathrm{\θ}}\left(N\right)+{\mathrm{\ΔJ}}_N\left(M\right)\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V\left(M\right)\\ =\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\ω}}_e\left(N\right)\end{array}\right]+{\mathrm{\ΔJ}}_N\left(M\right)\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V\left(M\right)\end{array}---\left(19\right)$

其中

$E_V\left(M\right)=J_g\left(N\right)E_{\mathrm{\θ}}\left(M\right)+\mathrm{\ΔJ}\left(M\right)E_{\mathrm{\θ}}+E_J\left(M\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)---\left(20\right)$

当前时刻为N时刻，视觉测量时延为P个采样周期，根据式（19），得N-P时刻的末端速度为：

$\begin{array}{c}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\ω}}_e(N-P)\end{array}\right]=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\ω}}_e\left(N\right)\end{array}\right]\\ +{\mathrm{\ΔJ}}_N(N-P)\frac{\stackrel{\·}{\mathrm{\θ}}(N-P)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V(N-P)\end{array}---\left(21\right)$

根据式（19），得N-P-1时刻的末端速度为：

$\begin{array}{c}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\ω}}_e(N-P-1)\end{array}\right]=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P-1)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\ω}}_e\left(N\right)\end{array}\right]\\ +{\mathrm{\ΔJ}}_N(N-P-1)\frac{\stackrel{\·}{\mathrm{\θ}}(N-P-1)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V(N-P-1)\end{array}---\left(22\right)$

而N-P-1时刻对于N时刻的目标粗估计矩阵△J_N可表示为：

$\begin{array}{c}{\mathrm{\ΔJ}}_N(N-P-1)\\ =\left[\frac{{\∂J}_{g1}\left(N\right)}{\∂{\mathrm{\θ}}_1}\right[{\mathrm{\θ}}_1(N-P-1)-{\mathrm{\θ}}_1\left(N\right)]\·\·\·\frac{{\∂J}_{\mathrm{gn}}\left(N\right)}{{\∂\mathrm{\θ}}_n}[{\mathrm{\θ}}_n(N-P-1){\mathrm{\θ}}_n\left(N\right)\left]\right]\\ \frac{[{\mathrm{\θ}}_1(N-P-1)-{\mathrm{\θ}}_1\left(N\right)]\·[{\mathrm{\θ}}_1(N-P)-{\mathrm{\θ}}_1\left(N\right)]}{{\left|\right[{\mathrm{\θ}}_1(N-P)-\left]{\mathrm{\θ}}_1\left(N\right)\right]|}_2^2}{\mathrm{\ΔJ}}_N(N-P)+T_{\mathrm{\ΔJ}}\end{array}---\left(23\right)$

其中T_△J为余项。根据以上的结论可以得到当前末端速度为

$\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\ω}}_e\left(N\right)\end{array}\right]=\frac{-\mathrm{\β}}{{\mathrm{\α}}_1(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\ω}}_e(N-P)\end{array}\right]+\frac{1}{{\mathrm{\α}}_2(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\ω}}_e(N-P-1)\end{array}\right]+\mathrm{\ΔE}---\left(24\right)$

其中

$\left\{\begin{array}{c}\mathrm{\β}=\frac{[{\mathrm{\θ}}_1(N-P-1)-{\mathrm{\θ}}_1\left(N\right)]\·[{\mathrm{\θ}}_1(N-P)-{\mathrm{\θ}}_1\left(N\right)]}{{\left|\right[{\mathrm{\θ}}_1(N-P)-{\mathrm{\θ}}_1\left(N\right)\left]\right|}_2^2}\\ {\mathrm{\α}}_1=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\right|}_2^2}\\ {\mathrm{\α}}_2=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P-1)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\end{array}\right.---\left(25\right)$

为了方便，可称α₁、α₂为二阶线性估计系数，β为状态差异项。忽略高阶误差△E，此时末端速度可以近似计算为

$\left[\begin{array}{c}{\tilde{v}}_e\left(N\right)\\ {\widetilde{\mathrm{\ω}}}_e\left(N\right)\end{array}\right]=\frac{-\mathrm{\β}}{{\mathrm{\α}}_1(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\ω}}_e(N-P)\end{array}\right]+\frac{1}{{\mathrm{\α}}_2(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\ω}}_e(N-P-1)\end{array}\right]---\left(26\right)$

此时的估计误差为

$\mathrm{\ΔE}=\frac{-\mathrm{\β}}{{\mathrm{\α}}_1(1-\mathrm{\β})}{E}_V(N-P)+\frac{1}{{\mathrm{\α}}_2(1-\mathrm{\β})}[T_{\mathrm{\ΔJ}}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V(N-P-1)]---\left(27\right)$

此时的估计速度可以计算为

$\begin{array}{c}{{\tilde{V}}_r\left(k\right)=\left[\begin{array}{cccccc}{\stackrel{\·}{x}}_r\left(k\right)& {\stackrel{\·}{y}}_r\left(k\right)& {\stackrel{\·}{z}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\α}}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\β}}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\γ}}}_r\left(k\right)\end{array}\right]}^T\\ \left[\begin{array}{c}{v}_e\\ ={\left[\begin{array}{ccc}0& -\sin {\mathrm{\α}}_r& \cos {\mathrm{\α}}_r\cos {\mathrm{\β}}_r\\ 0& \cos {\mathrm{\α}}_r& \sin {\mathrm{\α}}_r{\cos \mathrm{\β}}_r\\ 1& 0& -\sin {\mathrm{\β}}_r\end{array}\right]}^{-1}{\mathrm{\ω}}_e\end{array}\right]\end{array}.---\left(28\right)$

在测量时延较小、空间机器人运动较平稳的情况下，直接进行估计的误差△E应该很小，而在其他情况下估计误差△E也可能较大。针对估计误差较大的特殊情况，设计一个误差控制器，对估计值进行实时的校正，从而提高估计精度。

具体实施方式四：本实施方式与具体实施方式三的不同点是：本实施方式在步骤三所述的误差控制器设计为：将步骤二所述的估计速度序列的过程看作一个带有误差的超前环节：

$P\left(s\right)={\tilde{e}}^{{\mathrm{st}}_s}=e^{{\mathrm{st}}_s}[1+H_{\mathrm{\Δ}}\left(s\right)]---\left(29\right)$

其中

为超前环节的误差传递函数，视觉时延误差控制系统输出的传递函数为：

$\begin{array}{c}Y\left(s\right)=\frac{e^{{-\mathrm{st}}_s}{\tilde{e}}^{{\mathrm{st}}_s}+G\left(s\right)e^{{-\mathrm{st}}_s}}{1+G\left(s\right)e^{{\mathrm{st}}_s}}{V}_r\left(s\right)\\ =V_r\left(s\right)+\frac{H_{\mathrm{\Δ}}\left(s\right)}{1+G\left(s\right)e^{-{\mathrm{st}}_s}}{V}_r\left(s\right)\end{array}---\left(30\right)$

其中V_r(s)为机器人末端速度的真实值，Y(s)为校正后机器人末端速度估值，G(s)为误差控制器。

系统的估计误差传递函数为：

$E\left(s\right)\frac{-H_{\mathrm{\Δ}}\left(s\right)e^{{-\mathrm{st}}_s}-G\left(s\right)e^{{-2\mathrm{st}}_s}}{1+G\left(s\right)e^{-s{t}_s}}{V}_r\left(s\right)---\left(31\right)$

通过极点配置，设计使E(s)稳定的误差控制器G(s)，估计误差E(t)随时间逐渐收敛，校正后机器人末端速度估值逐渐趋近于速度真实值。

具体实施方式五：本实施方式与具体实施方式四的不同点是：本实施方式在步骤四所述的所述的根据校正后空间机器人末端速度估值，对当前带有时延的视觉测量数据进行补偿，经过补偿的视觉测量数据计算如下：

${\tilde{D}}_r\left(N\right)=D_v\left(N\right)+t_s\×\underset{i=1}{\overset{m}{\mathrm{\Σ}}}Y(N-i)---\left(32\right)$

其中Y(N-i)是步骤四得出的校正后空间机器人末端速度估值，至此完成了视觉时延误差的补偿。

本发明说明书中未做详细描述的内容属于本领域专业技术人员公知的现有技术。

实施例

结合图1、图2、图3说明本实施例，实验系统由空间机器人、目标卫星组成，空间机器人包括六自由度机械臂和载体航天器，机械臂末端安装相机。机械臂带相机跟踪接近目标星，目标卫星以4mm/s的速度沿机械臂末端Z轴方向运动。

为建立仿真系统，载体航天器和六自由度机械臂的运动学和动力学参数如表1所示

表1.空间机器人参数

空间机器人的视觉时延误差补偿步骤为：

步骤一、根据所采用的视觉信息处理方法和所应用的硬件环境确定视觉系统的时延，根据公式错误！未找到引用源。建立真实距离信息和视觉测量信息间的数学关系。

步骤二、利用公式(26)计算当前时刻空间机器人末端速度估计。

步骤三、根据公式(30)设计视觉时延误差控制器，经控制器输出，得到校正后空间机器人末端速度估值。

步骤四、利用公式(32)计算补偿后的视觉测量数据。

所设计空间机器人视觉时延误差补偿实际效果如图4、图5所示。由图4的补偿后视觉测量的估计数据与原始测量数据和真实相对位置对比图，可以看出，由于视觉时延的存在，在X、Y和Z三个方向上，视觉系统对目标的测量值与真实值存在较大的误差，经过误差补偿，视觉测量的估计数据非常接近真实值。由图5的补偿后视觉测量数据估计误差与未补偿前视觉测量数据时延误差的对比图，可以看出，补偿前视觉测量与真实值的最大误差，在X方向由18mm减小到7mm，在Z向由38mm减小到8mm，可以看出本发明的这种时延补偿方法，可以明显减小漂浮基座下空间机器人由于时延引起的视觉测量误差，有利于更好地保证空间机器人在轨高精度地完成精细操作任务的要求，并且整个过程计算简单，不需要建立空间机器人精确的数学模型，不需进行复杂的动力学计算，能满足工程实际需求。

Claims (5)

1.一种基于速度估计的空间机器人视觉时延误差补偿方法，其特征在于：所述方法由以下步骤完成：

步骤一、根据所采用的视觉处理算法和所应用的硬件确定视觉系统的视觉时延，建立带时延的视觉测量数据与物理真实相对位姿间的数学关系；

步骤二、根据带有时延的视觉测量数据和机械臂的关节指令，估计当前空间机器人的末端速度；

步骤三、设计误差控制器，减小空间机器人末端速度估值的误差，得到校正后空间机器人末端速度估值；

步骤四、根据校正后空间机器人末端速度估值，对当前带有时延的视觉测量数据进行补偿，得到经过补偿的视觉测量数据。

2.根据权利要求1所述的一种基于速度估计的空间机器人视觉时延误差补偿方法，其特征在于：

在步骤一中，根据所采用的视觉处理算法和所应用的硬件确定视觉系统的时延，建立带时延的视觉测量数据与物理真实相对位姿（测量相机与目标）间的数学关系，其过程为：

根据所采用的视觉信息处理方法和所应用的硬件平台确定整个视觉测量环节引起的时延为m个周期，若系统每个周期的时长为t_s，则空间机器人系统的视觉时延T_d为：

T_d=m×t_s              (1)

定义空间机器人k时刻的视觉测量信息D_v(k)和真实距离信息D_r(k)为：

D_v(k)=[x_v(k)y_v(k)z_v(k)α_v(k)β_v(k)γ_v(k)]^T   (2)

D_r(k)=[x_r(k)y_r(k)z_r(k)α_r(k)β_r(k)γ_r(k)]^T   (3)

其中k为通用时刻，x(k)，_y(k)，z(k)为相对位置信息，α(k)，β(k)，γ(k)为描述相对位姿的欧拉角，空间机器人视觉测量信息D_v(k)和真实距离信息D_r(k)间的关系为：

D_r(k)=D_v(k+m)    (4)

k表示任意时刻，m表示时延；

若当前时刻为N，视觉测量信息为D_v(N)，则可知道D_r(N-m)之前的真实距离信息都可由过去时刻的视觉测量数据直接得到。定义真实速度信息V_r(k)为：

$V_r\left(k\right)={\left[\begin{array}{cccccc}{\stackrel{\·}{x}}_r\left(k\right)& {\stackrel{\·}{y}}_r\left(k\right)& {\stackrel{\·}{z}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\α}}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\β}}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\γ}}}_r\left(k\right)\end{array}\right]}^T---\left(5\right)$

由D_r(k+1)和D_r(k)计算得到

$V_r\left(k\right)=\frac{D_r(k+1)-D_r\left(k\right)}{t_s}---\left(6\right)$

空间机器人的真实速度信息V_r(N-m-1)和V_r(N-m-1)之前的真实速度信息通过计算得到：真实的距离信息D_k(N)为

$D_r\left(N\right)=D_v\left(N\right)+t_s\×\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{V}_r(N-i)---\left(7\right)$

3.根据权利要求1或2所述的一种基于速度估计的空间机器人视觉时延误差补偿方法，其特征在于：在步骤二中，所述的根据带有时延的视觉测量数据和机械臂的关节指令，估计当前空间机器人的末端速度，其过程为：

利用带有时延的视觉测量数据，得到真实速度序列的估值序列

则可通过速度估值序列

对当前的视觉测量信息D_v(N)进行补偿，式(7)改写为：

${\tilde{D}}_r\left(N\right)=D_v\left(N\right)+t_s\×\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{\tilde{V}}_r(N-i)---\left(8\right)$

从而得到当前的真实距离信息的估计值

将带有时延的空间机器人视觉测量数据与机械臂的关节指令相结合，构造出当前时刻的空间机器人末端速度的估值；

由于处于零重力状态下的空间机器人满足动量守恒和角动量守恒，依据此建立空间机器人关节运动状态与末端效应器广义速度间的数学关系，满足方程：

$\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]=J_g({\mathrm{\Ψ}}_0,\mathrm{\Θ},m_i,I_i)\stackrel{\·}{\mathrm{\Θ}}=\left[\begin{array}{c}{J}_{g\_v}\\ J_{g\_\mathrm{\ω}}\end{array}\right]\stackrel{\·}{\mathrm{\Θ}}---\left(9\right)$

其中参数矩阵J_g就是空间机器人的广义雅克比矩阵，是惯量矩阵I_i、质量参数m_i和机器人关节角Θ、载体航天器姿态Ψ₀的函数，

是机器人的关节角速度；其中

$v_e={\left[\begin{array}{ccc}{\stackrel{\·}{x}}_r& {\stackrel{\·}{y}}_r& {\stackrel{\·}{z}}_r\end{array}\right]}^T---\left(10\right)$

${\mathrm{\ω}}_e=\left[\begin{array}{ccc}0& -\sin {\mathrm{\α}}_r& \cos {\mathrm{\α}}_r\cos {\mathrm{\β}}_r\\ 0& \cos {\mathrm{\α}}_r& \sin {\mathrm{\α}}_r\cos {\mathrm{\β}}_r\\ 1& 0& -\sin {\mathrm{\β}}_r\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\α}}}_r\\ {\stackrel{\·}{\mathrm{\β}}}_r\\ {\stackrel{\·}{\mathrm{\γ}}}_r\end{array}\right]---\left(11\right)$

v_e和ω_e分别是空间机器人末端执行器的线速度和角速度，

和

为空间机器人末端姿态欧拉角的微分；

将广义雅可比矩阵进行展开，考虑空间机器人的末端广义速度：

$\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{J}_{\mathrm{gi}}{\mathrm{\θ}}_i---\left(12\right)$

其中J_gi是广义雅克比矩阵的第i列，θ_i是第i个机器人关节角；

因空间机器人各运动学、动力学参数的真实值都包含在带时延的测量数据中，定义M时刻在当前时刻N的邻域内，将J_gi(M)展开为：

$J_{\mathrm{gi}}\left(M\right)=J_{\mathrm{gi}}\left(N\right)+\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\underset{j=1}{\overset{\∞}{\mathrm{\Σ}}}\frac{1}{j!}\frac{{\∂}^j{J}_{\mathrm{gi}}}{{\∂\mathrm{\θ}}_i^j}{[{\mathrm{\θ}}_i\left(M\right)-{\mathrm{\θ}}_i\left(N\right)]}^j+E_{\mathrm{Ji}}^{\′}\left(M\right)---\left(13\right)$

其中E′_Ji(M)为因载体姿态变化引起的误差；仅保留其中的一次项，进一步变形为

$\left\{\begin{array}{c}{J}_{\mathrm{gi}}\left(M\right)=J_{\mathrm{gi}}\left(N\right)+\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\frac{\∂J_{\mathrm{gi}}\left(N\right)}{{\∂\mathrm{\θ}}_i}[{\mathrm{\θ}}_i\left(M\right)-{\mathrm{\θ}}_i\left(N\right)]+E_{\mathrm{Ji}}\left(M\right)\\ E_{\mathrm{Ji}}\left(M\right)=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\underset{j=2}{\overset{\∞}{\mathrm{\Σ}}}\frac{1}{j!}\frac{{\∂}^j{J}_{\mathrm{gi}}\left(N\right)}{{\∂\mathrm{\θ}}_i^j}{[{\mathrm{\θ}}_i\left(M\right)-{\mathrm{\θ}}_i\left(N\right)]}^j+E_{\mathrm{Ji}}^{\′}\left(M\right)\end{array}\right.---\left(14\right)$

此时M时刻的空间机器人末端速度可表示为

$\begin{array}{c}\left[\begin{array}{c}{v}_e\left(M\right)\\ {\mathrm{\ω}}_e\left(M\right)\end{array}\right]=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{J}_{\mathrm{gi}}\left(M\right){\stackrel{\·}{\mathrm{\θ}}}_i\left(M\right)\\ =J_g\left(N\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)+{\mathrm{\ΔJ}}_N\left(M\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)+E_J\left(M\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)\end{array}---\left(15\right)$

其中

${\mathrm{\ΔJ}}_N\left(M\right)=\left[\frac{\∂J_{g1}\left(N\right)}{{\∂\mathrm{\θ}}_1}\right[{\mathrm{\θ}}_1\left(M\right)-{\mathrm{\θ}}_1\left(N\right)]\·\·\·\frac{{\∂J}_{\mathrm{gn}}\left(N\right)}{{\∂\mathrm{\θ}}_n}[{\mathrm{\θ}}_n\left(M\right)-{\mathrm{\θ}}_n\left(N\right)\left]\right]---\left(16\right)$

M时刻的空间机器人关节角速度表述为：

$\stackrel{\·}{\mathrm{\θ}}\left(M\right)=\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_{\mathrm{\θ}}\left(M\right)---\left(17\right)$

其中

$E_{\mathrm{\θ}}\left(M\right)=\stackrel{\·}{\mathrm{\θ}}\left(M\right)-\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)---\left(18\right)$

由于空间机器人运动时，关节角速度变化比较慢，所以公式(17)第一项为M时刻空间机器人关节角速度的主要部分，E_θ(M)为与加速度相关的余项；

将式（17）带入式（15），得M时刻的空间机器人末端速度表示为：

$\begin{array}{c}\left[\begin{array}{c}{v}_e\left(M\right)\\ {\mathrm{\ω}}_e\left(M\right)\end{array}\right]=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{J}_{\mathrm{gi}}\left(M\right){\stackrel{\·}{\mathrm{\θ}}}_i\left(M\right)\\ =\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}{J}_g\left(N\right)\stackrel{\·}{\mathrm{\θ}}\left(N\right)+{\mathrm{\ΔJ}}_N\left(M\right)\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V\left(M\right)\\ =\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\ω}}_e\left(N\right)\end{array}\right]+{\mathrm{\ΔJ}}_N\left(M\right)\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V\left(M\right)\end{array}---\left(19\right)$

其中

$E_V\left(M\right)=J_g\left(N\right)E_{\mathrm{\θ}}\left(M\right)+\mathrm{\ΔJ}\left(M\right)E_{\mathrm{\θ}}+E_J\left(M\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)---\left(20\right)$

当前时刻为N时刻，视觉测量时延为P个采样周期，根据式（19），得N-P时刻的末端速度为：

$\begin{array}{c}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\ω}}_e(N-P)\end{array}\right]=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\ω}}_e\left(N\right)\end{array}\right]\\ +{\mathrm{\ΔJ}}_N(N-P)\frac{\stackrel{\·}{\mathrm{\θ}}(N-P)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V(N-P)\end{array}---\left(21\right)$

根据式（19），得N-P-1时刻的末端速度为：

$\begin{array}{c}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\ω}}_e(N-P-1)\end{array}\right]=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P-1)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\ω}}_e\left(N\right)\end{array}\right]\\ +{\mathrm{\ΔJ}}_N(N-P-1)\frac{\stackrel{\·}{\mathrm{\θ}}(N-P-1)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V(N-P-1)\end{array}---\left(22\right)$

而N-P-1时刻对于N时刻的目标粗估计矩阵△J_N可表示为：

$\begin{array}{c}{\mathrm{\ΔJ}}_N(N-P-1)\\ =\left[\frac{{\∂J}_{g1}\left(N\right)}{\∂{\mathrm{\θ}}_1}\right[{\mathrm{\θ}}_1(N-P-1)-{\mathrm{\θ}}_1\left(N\right)]\·\·\·\frac{{\∂J}_{\mathrm{gn}}\left(N\right)}{{\∂\mathrm{\θ}}_n}[{\mathrm{\θ}}_n(N-P-1){\mathrm{\θ}}_n\left(N\right)\left]\right]\\ \frac{[{\mathrm{\θ}}_1(N-P-1)-{\mathrm{\θ}}_1\left(N\right)]\·[{\mathrm{\θ}}_1(N-P)-{\mathrm{\θ}}_1\left(N\right)]}{{\left|\right[{\mathrm{\θ}}_1(N-P)-\left]{\mathrm{\θ}}_1\left(N\right)\right]|}_2^2}{\mathrm{\ΔJ}}_N(N-P)+T_{\mathrm{\ΔJ}}\end{array}---\left(23\right)$

其中T_△J为余项，根据式(19)、式(21)和式(22)，可以得到当前N时刻的末端速度为：

$\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\ω}}_e\left(N\right)\end{array}\right]=\frac{-\mathrm{\β}}{{\mathrm{\α}}_1(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\ω}}_e(N-P)\end{array}\right]+\frac{1}{{\mathrm{\α}}_2(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\ω}}_e(N-P-1)\end{array}\right]+\mathrm{\ΔE}---\left(24\right)$

其中

$\left\{\begin{array}{c}\mathrm{\β}=\frac{[{\mathrm{\θ}}_1(N-P-1)-{\mathrm{\θ}}_1\left(N\right)]\·[{\mathrm{\θ}}_1(N-P)-{\mathrm{\θ}}_1\left(N\right)]}{{\left|\right[{\mathrm{\θ}}_1(N-P)-{\mathrm{\θ}}_1\left(N\right)\left]\right|}_2^2}\\ {\mathrm{\α}}_1=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\right|}_2^2}\\ {\mathrm{\α}}_2=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P-1)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{{\left|\stackrel{\·}{\mathrm{\θ}}\left(N\right)\right|}_2^2}\end{array}\right.---\left(25\right)$

α₁、α₂为二阶线性估计系数，β为状态差异项；忽略高阶误差△E，此时末端速度可近似计算为

$\left[\begin{array}{c}{\tilde{v}}_e\left(N\right)\\ {\widetilde{\mathrm{\ω}}}_e\left(N\right)\end{array}\right]=\frac{-\mathrm{\β}}{{\mathrm{\α}}_1(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\ω}}_e(N-P)\end{array}\right]+\frac{1}{{\mathrm{\α}}_2(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\ω}}_e(N-P-1)\end{array}\right]---\left(26\right)$

估计误差为：

$\mathrm{\ΔE}=\frac{-\mathrm{\β}}{{\mathrm{\α}}_1(1-\mathrm{\β})}{E}_V(N-P)+\frac{1}{{\mathrm{\α}}_2(1-\mathrm{\β})}[T_{\mathrm{\ΔJ}}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V(N-P-1)]---\left(27\right)$

根据式(10)和式(11)，估计速度为：

$\begin{array}{c}{{\tilde{V}}_r\left(k\right)=\left[\begin{array}{cccccc}{\stackrel{\·}{x}}_r\left(k\right)& {\stackrel{\·}{y}}_r\left(k\right)& {\stackrel{\·}{z}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\α}}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\β}}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\γ}}}_r\left(k\right)\end{array}\right]}^T\\ \left[\begin{array}{c}{v}_e\\ ={\left[\begin{array}{ccc}0& -\sin {\mathrm{\α}}_r& \cos {\mathrm{\α}}_r\cos {\mathrm{\β}}_r\\ 0& \cos {\mathrm{\α}}_r& \sin {\mathrm{\α}}_r{\cos \mathrm{\β}}_r\\ 1& 0& -\sin {\mathrm{\β}}_r\end{array}\right]}^{-1}{\mathrm{\ω}}_e\end{array}\right]\end{array}.---\left(28\right)$

4.根据权利要求3所述的一种基于速度估计的空间机器人视觉时延误差补偿方法，其特征在于：在步骤三中，所述的误差控制器设计为：

将步骤二所述的估计速度序列的过程看作一个带有误差的超前环节：

$P\left(s\right)={\tilde{e}}^{{\mathrm{st}}_s}=e^{{\mathrm{st}}_s}[1+H_{\mathrm{\Δ}}\left(s\right)]---\left(29\right)$

其中

为超前环节的误差传递函数，视觉时延误差控制系统输出的传递函数为：

$\begin{array}{c}Y\left(s\right)=\frac{e^{{-\mathrm{st}}_s}{\tilde{e}}^{{\mathrm{st}}_s}+G\left(s\right)e^{{-\mathrm{st}}_s}}{1+G\left(s\right)e^{{\mathrm{st}}_s}}{V}_r\left(s\right)\\ =V_r\left(s\right)+\frac{H_{\mathrm{\Δ}}\left(s\right)}{1+G\left(s\right)e^{-{\mathrm{st}}_s}}{V}_r\left(s\right)\end{array}---\left(30\right)$

其中V_r(s)为机器人末端速度的真实值，Y(s)为校正后机器人末端速度估值，G(s)为误差控制器；

系统的估计误差传递函数为：

$E\left(s\right)\frac{-H_{\mathrm{\Δ}}\left(s\right)e^{{-\mathrm{st}}_s}-G\left(s\right)e^{{-2\mathrm{st}}_s}}{1+G\left(s\right)e^{-s{t}_s}}{V}_r\left(s\right)---\left(31\right)$

通过极点配置，设计使E(s)稳定的误差控制器G(s)，估计误差E(t)随时间逐渐收敛，校正后机器人末端速度估值逐渐趋近于速度真实值。

5.根据权利要求4所述的一种基于速度估计的空间机器人视觉时延误差补偿方法，其特征在于：在步骤四中，所述的根据校正后空间机器人末端速度估值，对当前带有时延的视觉测量数据进行补偿，经过补偿的视觉测量数据

(N)计算如下：

${\tilde{D}}_r\left(N\right)=D_v\left(N\right)+t_s\×\underset{i=1}{\overset{m}{\mathrm{\Σ}}}Y(N-i)---\left(32\right)$

其中Y(N-i)是步骤四得出的校正后空间机器人末端速度估值，至此完成了视觉时延误差的补偿。

Images