一种空间绳系机器人视觉精确逼近方法与流程

本发明属于航天技术领域，涉及一种空间绳系机器人视觉精确逼近方法，是一种基于模型预测控制的空间绳系机器人视觉精确逼近目标的方法。

背景技术：

在辅助变轨、轨道垃圾清理、失控卫星救助等方面，空间机器人拥有自己独特的优势。本发明所研究的对象主要针对带绳系的空间机器人，也可以进行稍微改进应用到其它类型空间机器人当中。空间绳系机器人由“平台卫星+空间系绳+操作机器人”三部分构成，如图1。空间绳系机器人的操作机器人是逼近并抓捕目标的具体单元。对于操作机器人来说，实时测量其与目标之间的相对位置是操作机器人精确逼近目标导航任务完成的关键。相对平台本身，操作机器人一般较小，无法搭载重量较大的高性能外部感知传感器，如激光测距仪等。因此，视觉传感器是其最常见外部感知传感器，负责操作机器人逼近目标时的相对位置导航测量。但是众所周知，单目视觉传感器可以精确测量目标和操作机器人之间的相对角度，但对于相对距离测量有很大局限性。因此在现有的空间绳系机器人逼近方法中，单目相机一般只起到目标角度对准的作用，不进行距离测量。在必须需要距离信息时，单目相机需要结合目标先验尺寸和几何模型才能获得操作机器人与目标之间的相对距离测量信息。而对于非合作目标，这就要求平台提前进行目标绕飞，进行目标精确建模等前提条件，这就显然给绳系机器人平台又提出了额外的要求。

技术实现要素：

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种空间绳系机器人视觉精确逼近方法，通过实时优化逼近速度和方向，使操作机器人仅利用单目相机就可以对目标进行精确的距离测量并且逼近目标，解决现有方法的局限性。

技术方案

一种空间绳系机器人视觉精确逼近方法，其特征在于：待抓捕目标的本体坐标系为O_txyz，O_w和O分别为机器人卫星平台和操作机器人的中心，操作机器人质量为m，卫星平台和操作机器人中心在待抓捕目标本体坐标系中的相对位置分别为O_w＝(x_w y_w z_w)^T和O＝(x y z)^T，待抓捕目标相对于操作机器人的方位角定义为α、俯仰角定义为β，F_t为卫星平台对操作机器人的系绳张力，推进器总推力为F_s，其中F_sx，F_sy，F_sz是F_s沿着x,y,z三坐标轴的分量，相机成像平面的横、纵坐标分别为u、v，相机的焦距为f；逼近步骤如下：

步骤1：对操作机器人进行动力学建模，得到空间绳系机器人的系统动力学方程和相机观测方程：

系统动力学方程

其中：n是轨道角速度。

相机观测方程为：

步骤2：采用扩展型卡尔曼滤波器EKF对系统动力学方程和相机观测方程进行滤波，得到操作机器人与目标之间相对位置的估计值和评价估计精度的协方差矩阵；

步骤3：利用伪谱法，基于系统动力学方程，得到基于连续控制力的空间绳系机器人逼近目标的离线轨迹规划；

步骤4：使用模型预测控制MPC方法，对步骤3得到的轨迹规划跟踪精度作为一个总体优化目标，对步骤2得到的估计精度作为另一个优化目标，控制操作机器人的运动方向和运动速度，实现对待抓捕目标的精确的逼近。

有益效果

本发明提出的一种空间绳系机器人视觉精确逼近方法，利用相机对目标得到的角度信息，建立基于卡尔曼滤波器的状态估计，并利用模型预测控制算法控制操作机器人的方向和速度，将滤波器的状态估计和路径跟踪联合最优，从而使得空间操作机器人一方面对目标进行逼近，另一方面单目测量得到精度保证的相对位置估计。本发明的有益效果是，使用基于模型预测控制的空间绳系机器人单目精确逼近方法，在仅使用单目相机的情况下，就能实现了空间绳系机器人对目标的实时精确定位，并准确的逼近目标。

附图说明

图1是空间绳系机器人系统图；

图2是包含卫星平台、系绳、空间机器人及目标的系统坐标定义图；

图3是空间操作机器人的受力图；

图4是相机观测目标成像原理图；

图5是控制系统结构图。

具体实施方式

现结合实施例、附图对本发明作进一步描述：

本发明所采用的技术方案是：一种基于模型预测控制(MPC)的空间绳系机器人精确逼近目标方法。对绳系机器人首先进行动力学建模，将动力学方程作为操作机器人与目标之间相对位置估计的系统方程，将单目相机对待抓捕目标的角度测量信息作为观测方程。基于上述系统方程和观测方程，可利用扩展型卡尔曼滤波器进行相对位置的实时估计。最终，使用模型预测控制方法在线实时调整操作机器人的逼近方向和速度，使得操作机器人一方面沿着预定轨迹逼近目标，另一方面保证相对位置估计满足一定精度要求。

具体按照以下步骤实施：

步骤1：对操作机器人进行动力学建模，得到空间绳系机器人的系统动力学方程和相机观测方程。

步骤2：采用扩展型卡尔曼滤波器EKF对系统动力学方程和相机观测方程进行滤波，得到操作机器人与目标之间相对位置的估计值和评价估计精度的协方差矩阵。

步骤3：利用伪谱法，基于系统动力学方程，得到基于连续控制力的空间绳系机器人逼近目标的离线轨迹规划。

步骤4：使用模型预测控制MPC方法，对步骤3得到的轨迹规划跟踪精度作为一个总体优化目标，对步骤2得到的估计精度作为另一个优化目标，控制操作机器人的运动方向和运动速度，实现对待抓捕目标的精确的逼近。

进一步的，空间绳系机器人系统示意图见图1所示，系统主要包括卫星平台、系绳、操作机器人。待抓捕目标位于系统前方。图2描述系统坐标定义图。定义待抓捕目标的本体坐标系为O_txyz，空间机器人的逼近导航在此坐标系下进行描述。定义O_w和O分别为机器人卫星平台和操作机器人的中心，操作机器人质量为m，卫星平台和操作机器人中心在待抓捕目标本体坐标系中的相对位置分别为O_w＝(x_w y_w z_w)^T和O＝(x y z)^T，待抓捕目标相对于操作机器人的方位角定义为α、俯仰角定义为β。空间机器人的受力分析如图3所示，F_t为卫星平台对操作机器人的系绳张力，推进器总推力为F_s，其中F_sx，F_sy，F_sz是F_s沿着x,y,z三坐标轴的分量，相机成像平面的横、纵坐标分别为u、v，相机的焦距为f。

步骤1主要对操作机器人进行动力学建模，得到系统状态方程和相机观测方程。本发明假设平台重量相对较大，操作机器人的逼近机动不会影响卫星平台的运动，因此不需要对平台进行建模。此外，在本发明中，操作机器人的姿态假设在高精度陀螺仪控制下总是精确朝向x轴，因此本专利不考虑姿态问题。基于此假设，根据图2中所示的角度关系，操作机器人的系统的动力学方程可以建立为：

其中n是轨道角速度。

使用相机对目标卫星进行观测，根据图2和图4定义，由针孔相机成像原理可以推导出：继而得到相机观测方程为：

步骤2是利用步骤1得到的系统动力学方程和相机观测方程，使用扩展型卡尔曼滤波器(EKF)实现操作机器人与目标之间相对位置估计，并得到评价估计精度的协方差矩阵。这里需要对系统首先进行离散化。将第k时刻的表述操作机器人的离散时间系统状态(位置+速度)定义为：

其中x_k表示变量x在离散时间k时刻的取值，表示x_k的一阶导数，类似标注方式应用于本专利其它变量。式(1)系统方程也可以重新组织为离散形式：

X_k+1＝X_k+T·f_k (4)

其中T是离散采样时间间隔，类似定义适用于整个申请书。式(4)中非线性函数f_k为：

根据式(2)，定义系统k时刻的离散观测值为Y_k＝[u_k v_k]，得到离散观测方程为：

下面使用传统的EKF滤波对公式(4)和(6)对应的离散系统进行状态估计：

1)第一个公式为状态转移方程，目的是得到EKF中状态预测

其中是第k步的EKF状态最优估计，代表进行了一步状态转移后的状态预测估计值,F_k是公式(5)的一阶泰勒展开；

2)第二个公式得到状态估计误差协方差P_k+1|k的转移方程，

P_k+1|k＝F_kP_k|kF_k^T+Q_k，其中P_k|k是第k步误差协方差估计，P_k+1|k是进行了一步状态转以后的误差协方差估计，Q_k为过程噪声方差矩阵；

3)第三个公式为卡尔曼增益K_k+1计算，

其中H_k是公式(6)非线性观测方程对状态变量的一阶泰勒展开系数矩阵；

4)第四个公式为滤波估计方程，根据观测值的状态更新，得到第k+1步的EKF状态最优估计

5)第五个是状态估计误差协方差矩阵更新方程，P_k+1|k+1＝[I-K_k+1H_k+1]P_k+1|k，其中P_k+1|k+1是第k+1步的误差协方差估计更新，I是单位矩阵。

步骤3基于步骤1中的系统动力学方程和观测方程，利用伪谱法离线计算基于连续控制力的操作机器人轨迹规划。令及U_a＝[a_x a_y a_z]^T，则可以得到简化的动力学模型状态方程为：

X_k+1＝AX_k+BU_ak (7)

U_ak是U_a在第k时刻的值，

0_3×3代表3行3列零矩阵，I_3×3代表3行3列单位矩阵。

使用高斯伪谱法对操作机器人在飞行时间为t∈[t₀,t_f]进行离线轨迹规划，规划的优化目标可以根据具体任务需要选定，t₀是逼近起始时间，t_f是期望结束时间。在高斯伪谱法中，连续控制力最优逼近轨迹求解问题的动力学微分模型约束可以化为6K维代数约束，K是轨迹采样点个数，且同时需要满足输入约束为-a_max≤a_i≤a_max(i＝x,y,z)。高斯伪谱法求解采用标准规划方法，不是专利发明，顾不再本专利叙述，得到连续控制力下的离线轨迹规划。最后使用K个轨迹采样点进行差值，得到任意离散k时刻离线轨迹规划的坐标表示为(x_ok y_ok z_ok)。

步骤4使用模型预测控制(MPC)方法，并将步骤3中得到的离线轨迹规划作为一个轨迹跟踪优化目标，以及步骤2中的实时位置估计精度作为另一个优化目标，控制操作机器人的方向和速度。

定义U_k为第k时刻的系统输入，其中U_k＝[F_tk F_sxk F_syk F_szk]^T。定义N为模型预测预测时域窗口；X_k是第k时刻状态矢量；X_k+N是预测时域窗口结束时第k+N时刻的结束状态矢量；U_K＝[U_k U_k+1 … U_k+N]为预测时域窗口内优化求解后的控制序列的集合。

对于每一个采样时刻k，MPC在线求解有限开环最优控制问题为:

且最优问题同时满足约束

其中，F_{x max}，F_{y max}，F_{z max}代表最大推进力值，J是优化目标函数，第一项为k+N时刻对X_k+N估计的Fisher信息矩阵Φ(X_k+N)的逆矩阵主对角元素和，即其中表示Fisher信息矩阵Φ(X_k+N)逆矩阵的第(i,i)个元素。Fisher信息矩阵Φ(X_k+N)可以遵循下面的迭代公式计算：

其中优化目标函数第二项中的λ₁为常数，是针对空间绳系机器人最优轨迹相关路径跟踪的一项优化

待模型预测控制优化结算出U_K后，其第一个元素U_k即可作为第k时刻真实控制输入控制系统。在第k+1时刻时，重复步骤4，直至操作机器人逼近目标。

具体实施例：

图1是空间绳系机器人系统图。系统由卫星平台、系绳、操作机器人构成。目标位于系统的前方。卫星平台为一个功能较多、质量和体积较大的卫星。当卫星平台逼近目标到一定距离时，就可以发射操作机器人对目标进行逼近和抓捕。操作机器人在飞行过程中通过安装在其上的单目相机实时的对其与目标之间的相对位置进行测量，引导操作机器人进行正确逼近机动。

图5是控制系统的反馈结构图，首先根据目标相对于操作机器人的角度信息，使用EKF方法估计空间操作机器人与目标的相对位置。然后将EKF得到的估计值反馈给模型预测控制，模型预测控制负责计算预测时域窗口内的最优输入，控制操作机器人的速度和方向。

图2是系统坐标定义图。定义待抓捕目标的本体坐标系为O_txyz，o_tx轴指向目标运动轨道的切线方向，o_tz轴指向地心，o_ty轴由右手定则确定，空间机器人的逼近导航在此坐标系下进行描述。定义O_w和O分别为机器人卫星平台和操作机器人的中心，操作机器人质量为m，卫星平台和操作机器人中心在待抓捕目标本体坐标系中的相对位置分别为O_w＝(x_w y_wz_w)^T和O＝(x y z)^T，待抓捕目标相对于操作机器人的方位角定义为α、俯仰角定义为β。空间机器人的受力分析如图3所示，F_t为卫星平台对操作机器人的系绳张力，推进器总推力为F_s，其中F_sx，F_sy，F_sz是F_s沿着x,y,z三坐标轴的分量，F_sx平行于x轴，F_sy平行于y轴，F_sz平行于z轴，相机成像平面的横、纵坐标分别为u、v，相机的焦距为f。

实现本发明提出的操作机器人单目精确逼近方法主要有一下4个步骤

步骤1：主要对操作机器人进行动力学建模，得到系统状态方程和相机观测方程。

在本发明中，操作机器人的姿态假设在高精度陀螺仪控制下总是精确朝向x轴，因此本专利不考虑姿态控制问题。基于此假设，根据图2中所示的角度关系，α、β可由卫星平台位置坐标(x_w y_w z_w)和操作机器人位置坐标(x y z)计算得到：空间绳系机器人系统的动力学方程可由空间相对导航经典的Clohesy-Wiltshire方程推导出：

n是轨道角速度。

使用操作机器人相机对目标卫星进行观测，根据图2和图4定义，由针孔相机成像原理可以推导出：继而得到相机观测方程为：

步骤2：是利用步骤1得到的系统动力学方程和相机观测方程，使用扩展型卡尔曼滤波器(EKF)实现操作机器人与目标之间相对位置估计，并得到评价估计精度的协方差矩阵。这将第k时刻的表述操作机器人的离散时间系统状态(位置+速度)定义为：

其中x_k表示变量x在离散时间k时刻的取值，表示x_k的一阶导数，类似标注方式应用于本专利其它变量。

式(1)系统方程也可以重新组织为离散形式：

X_k+1＝X_k+T·f_k (4)

其中T是离散采样时间间隔，类似定义适用于整个申请书。式(4)中非线性函数f_k为：

根据式(2)，定义系统k时刻的离散观测值为Y_k＝[u_k v_k]，得到相机离散观测方程为：

下面使用典型EKF滤波五个公式实现状态估计：

第一个公式为状态转移方程，目的是得到EKF中状态预测其中是第k步的EKF状态最优估计，代表进行了一步状态转移后的状态预测估计值,F_k是公式(5)的一阶泰勒展开系数：

其中

和

其中Δx＝x_k-x_w，Δy＝y_k-y_w，Δz＝z_k-z_w，I_3×3代表3行3列单位矩阵。

第二个公式是状态估计误差协方差P_k+1|k的转移方程，P_k+1|k＝F_kP_k|kF_k^T+Q_k，其中P_k|k是第k步误差协方差估计，P_k+1|k P_k|k是进行了一步状态转以后的误差协方差估计，Q_k为过程噪声方差矩阵，在本发明中假设Q_k＝0。

第三个公式为卡尔曼增益K_k+1计算，其作用是控制收敛速度，表达为：其中H_k是公式(6)非线性观测方程对状态变量的一阶泰勒展开系数矩阵，也就是

0_1×3代表1行3列零矩阵。

第四个公式为滤波估计方程，根据观测值的状态更新，得到第k+1步的EKF状态最优估计

第五个是状态估计误差协方差矩阵更新方程，P_k+1|k+1＝[I-K_k+1H_k+1]P_k+1|k，其中P_k+1|k+1是第k+1步的误差协方差估计更新，I是单位矩阵。

通过以上五个公式，就可以实时估计操作机器人和目标之间的相对位置。

步骤3：基于步骤2中的系统动力学方程和观测方程，利用伪谱法离线计算基于连续控制力的操作机器人轨迹规划。令及U_a＝[a_x a_y a_z]^T，则可以得到简化的动力学模型状态方程为：

X_k+1＝AX_k+BU_ak (9)

U_ak是U_a在第k时刻的值，

0_3×3代表3行3列零矩阵，I_3×3代表3行3列单位矩阵。

使用高斯伪谱法对操作机器人在飞行时间为t∈[t₀,t_f]进行离线轨迹规划，规划的优化目标可以根据具体任务需要选定，t₀是逼近起始时间，t_f是期望结束时间。连续控制力最优逼近轨迹求解问题的动力学微分模型约束可以化为6K维代数约束，K是轨迹采样点个数，且同时需要满足输入约束为-a_max≤a_i≤a_max(i＝x,y,z)。操作机器人推力器推力有限，规定推力器延坐标轴方向最大可以提供的加速度为a_max。高斯伪谱法求解采用标准规划方法，不是专利发明，顾不再本专利叙述，得到连续控制力下的离线轨迹规划。最后使用K个轨迹采样点进行差值，得到任意离散k时刻离线轨迹规划的坐标表示为(x_ok y_ok z_ok)。

步骤4：使用模型预测控制方法，并将步骤3中得到的离线轨迹规划作为一个轨迹跟踪优化目标，以及步骤2中的实时相对位置估计精度作为另一个优化目标，控制操作机器人的方向和速度。

定义U_k为第k时刻的系统输入，其中U_k＝[F_tk F_sxk F_syk F_szk]^T。定义N为模型预测预测时域窗口；X_k是第k时刻状态矢量；X_k+N是预测时域窗口结束时第k+N时刻的结束状态矢量；U_K＝[U_k U_k+1 … U_k+N]为预测时域窗口内优化求解后的控制序列的集合。

对于每一个采样时刻k，MPC在线求解有限开环最优控制问题为:

且最优问题同时满足约束

其中，其中，F_{x max}，F_{y max}，F_{z max}代表最大推进力值，J是优化目标函数，第一项为k+N时刻对X_k+N估计的Fisher信息矩阵Φ(X_k+N)的逆矩阵主对角元素和，即其中表示Fisher信息矩阵Φ(X_k+N)逆矩阵的第(i,i)个元素。Fisher信息矩阵Φ(X_k+N)可以遵循下面的迭代公式计算：

其中优化目标函数第二项中的λ₁为常数，是针对空间绳系机器人最优轨迹相关路径跟踪的一项优化

L₁(X(k+i))＝(x_k+i-x_o(k+i))²+(y_k+i-y_o(k+i))²+(z_k+i-z_o(k+i))²。

待模型预测控制优化结算出U_K后，其第一个元素U_k即可作为第k时刻真实控制输入控制系统。在第k+1时刻时，重复步骤4，直至操作机器人逼近目标。

本发明方法实现的关键是仅仅利用相机对目标得到的角度信息，建立基于卡尔曼滤波器的状态估计，并利用模型预测控制算法控制操作机器人的方向和速度，将滤波器的状态估计和路径跟踪联合最优，从而使得空间操作机器人一方面对目标进行逼近，另一方面单目测量得到精度保证的相对位置估计。