CN115619828A - 一种基于模拟双目视觉测量的空间机器人在轨捕获方法 - Google Patents

一种基于模拟双目视觉测量的空间机器人在轨捕获方法 Download PDF

Info

Publication number
CN115619828A
CN115619828A CN202211355555.9A CN202211355555A CN115619828A CN 115619828 A CN115619828 A CN 115619828A CN 202211355555 A CN202211355555 A CN 202211355555A CN 115619828 A CN115619828 A CN 115619828A
Authority
CN
China
Prior art keywords
target
plane
network
training
motion
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211355555.9A
Other languages
English (en)
Inventor
陈余军
安泉
藏悦
张尧
盛超
王兴龙
乐浪
刘正山
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China Academy of Space Technology CAST
Original Assignee
China Academy of Space Technology CAST
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by China Academy of Space Technology CAST filed Critical China Academy of Space Technology CAST
Priority to CN202211355555.9A priority Critical patent/CN115619828A/zh
Publication of CN115619828A publication Critical patent/CN115619828A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/08—Learning methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/292—Multi-camera tracking
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/90—Determination of colour characteristics
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20081—Training; Learning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20084—Artificial neural networks [ANN]

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Data Mining & Analysis (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computing Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Health & Medical Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Biomedical Technology (AREA)
  • Biophysics (AREA)
  • Computational Linguistics (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Molecular Biology (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种基于模拟双目视觉测量的空间机器人在轨捕获方法，本发明中，在仿真环境中模拟双目视觉相机，对立方体目标模块的特征点进行识别，基于拟合平面方法对目标模块位姿进行估计，提高观测精度；然后根据多帧图像下观测到的目标模块位姿，基于扩展卡尔曼滤波方法校正目标模块的运动状态，进而拟合目标模块的运动轨迹，实现对目标的运动预测；最后设计基于强化学习的空间机器人轨迹规划方法，以精确的空间机器人模型为训练环境，提高训练结果的真实性，设计能够引导机器人末端点到达期望抓捕点的奖励函数，加快训练过程，最终实现对目标模块的捕获。

Description

一种基于模拟双目视觉测量的空间机器人在轨捕获方法
技术领域
本发明涉及空间机器人在轨捕获技术领域，尤其涉及一种基于模拟双目视觉测量的空间机器人在轨捕获方法。
背景技术
随着航天技术的发展，建造大口径空间望远镜、大型空间电站等空间任务也逐渐获得国内外研究者的重视，但受限于当前运载火箭的运载能力，尚无法完成单一大型结构体的发射。为解决这一问题，大型空间结构朝着模块化的方向发展，将具备各种独立功能的空间模块发射入轨后，再利用空间机器人在轨组装这些模块，进而形成具有特定功能的大型结构，这种模块组装的形式也具有其它方面的优势，如当某些模块出现故障时，可直接对模块进行更换，提高了大型空间结构对故障的包容性和系统的可靠性。
目前现有的空间机器人在轨捕获过程中在轨捕获目标模块的能力存在较大的误差，不能达到目标位姿信息测量的效果，且对外界环境进行感知欠佳，不能对操作目标的运动状态进行校正，估计目标真实的运动状态，进而预测目标在未来某一时刻的运动情况，并且不能基于强化学习的空间机器人轨迹规划方法，以精确的空间机器人模型为训练环境，提高训练结果的真实性。
发明内容
本发明的目的在于：为了解决上述问题，而提出的一种基于模拟双目视觉测量的空间机器人在轨捕获方法。
为了实现上述目的，本发明采用了如下技术方案：
一种基于模拟双目视觉测量的空间机器人在轨捕获方法，包括以下步骤：
S1.基于模拟双目视觉相机获取目标模块的特征点信息，通过特征点匹配算法获取特征点的三维位置标记，进而按照拟合平面法解算目标模块的位置和姿态信息；
S2.根据步骤S1的目标识别过程，对目标模块进行持续观测和跟踪，基于EKF方法对目标的运动状态进行求解，得到目标运动的滤波估计结果，进而拟合目标模块的运动轨迹，实现对目标的运动预测；
S3.基于强化学习方法训练得到空间机器人的轨迹规划系统，将预测的目标位姿信息输入空间机器人的轨迹规划系统，实现对目标模块的快速捕获。
优选地，所述步骤S1中基于模拟双目视觉相机获取目标模块的特征点信息的方法包括以下步骤：
S11.在仿真环境下确定立方体目标模块的尺寸、颜色、指外法向量参数信息，作为后续模拟双目视觉相机的观测对象；
S12.将步骤S11中的立方体目标模块载入，由于目标在相机中的投影并非平行投影，而是汇聚于相机光心，因此在对目标进行拍摄图像模拟时，需要将惯性空间中，目标各顶点坐标转换为图像中的像素位置，还需要对图像中可以看到的平面进行判断和筛选，最终在图像中保留真正可以看到的平面。
S13.采用最小特征值算法检测图像的角点，在图像中能够检测到边缘、拐角特征位置的像素点；
S14.采用金字塔光流法进行左右相机视图特征点的匹配，求解所有匹配后的特征点对的三维空间坐标；
S15.对每一对匹配成功的点对，在各自的图像中进行颜色标记,以特征点为中心，选取一定像素范围，通过像素的RGB值进行判断，标记特征点附近像素点的颜色，将特征点附近出现过的颜色标记为该特征点的颜色，根据立方体目标的几何特征，一个特征点具有至少一个，至多三个颜色标记,
S16.依据带有颜色标签的特征点，寻找目标表面所在的平面，继而进行立方体目标的三维状态还原。
优选地，所述步骤S12的具体实现方法包括以下步骤：
S121.求解相机坐标系和像素坐标系下立方体目标上全部顶点的坐标，记为Xfc和Xfuv；
S122.进行第一次图像可视筛选，计算目标每个表面的指外法向量，指外法向量朝向相机一侧，即指外法向量和相机坐标系的Z轴的夹角大于90°，该平面在相机图像中才有可能可以看到，在平行投影的情况下，所有nout·zc＞0的平面都能够显示在相机图像中，其中nout为平面的指外法向量，zc为相机坐标系的Z轴方向，但由于相机采用透视成像，物体在投影时视野盲区变大，这使得相机图像中可以看到的平面的约束增强了，因此需要对进行第二次筛选；
S123.进行第二次图像可视筛选，确定在相机图像中真正可以看到的平面，将每个面的顶点按一定的顺序编号，使得顶点螺旋指向目标的外部，在像素平面上将这些顶点顺次连线，螺旋朝内则该平面能够在相机图像中看见，螺旋朝外则看到的是该平面的背面，对于实体目标，该平面在物体内部是看不见的。
优选地，所述步骤S16的实现方法具体包括以下步骤：
S161.定义某一个颜色平面的方程为：
ax+by+cz+d＝0 (1)；
提取具有相同颜色标签，对这些特征点所在平面进行线性拟合，拟合得到的平面函数为：
f(z)＝p00+p10 x+p01 y (2)；
其中三个系数与平面方程的四个参数的关系为：
[a b c d]＝[p10 p01 -1 p00] (3)；
S162.选取图像中检测到的不属于该平面的一个特征点，再从属于拟合平面的特征点中任选一个，建立从平面指向目标其他面上特征点的矢量n1，令所拟合平面的指外法向量为n0，两矢量之间的夹角为a0,当平面的法向量指向平面外，则a0必然为一钝角,当求解的平面法向量与特征点矢量之间夹角为钝角时，该平面参数是拟合平面的指外法向量，对不成钝角的情况则将平面方程前三个参数取负值；
S163.当得到多个平面的指外法向量后，将目标模块的质心位置和姿态角作为变量，设计指标函数并采用优化方法进行求解。指标函数写为如下形式：
Figure BDA0003921007000000041
其中rcenter代表惯性空间立方体质心位置矢量，
Figure BDA0003921007000000042
代表第i个特征点惯性空间坐标，n代表特征点的总数，L表示目标模块边长，
Figure BDA0003921007000000043
表示拟合的平面依据立方体边长平移后的过质心平面的法向量，
Figure BDA0003921007000000044
表示第k个拟合平面的指外法向量，m表示拟合的平面个数，AΘ表示从姿态角Θ时惯性系与本体系的3-1-2坐标转换矩阵，
Figure BDA0003921007000000045
则表示零姿态下对应颜色平面的指外法向量。
S164.对目标函数进行优化求解，得到目标模块质心位置和三轴姿态。
优选地，所述步骤S2中实现对目标的运动预测的方法包括以下步骤：
S21.扩展卡尔曼滤波将非线性系统中的非线性函数f(*)和h(*)围绕展开为泰勒级数，保留第一项，略去其他项，得到一个近似的一阶线性化模型，然后按照标准卡尔曼滤波的流程进行滤波估计，一般的非线性系统可以由以下方程描述；
Figure BDA0003921007000000046
Z(t)＝h[X(t),t]+v(t)； (5)；
式中W(t)和v(t)均是彼此不相关的零均值白噪声序列，他们与初始状态X(0)或X0也不相关，即对于t>t0有其统计学特性如下：
E[w(t)]＝0，E[w(t)·wT(τ)]＝q(t)δ(t-τ)
E[v(t)]＝0，E[v(t)·vT(τ)]＝r(t)δ(t-τ)
E[w(t)·vT(τ)]＝0，E[X(0)·wT(τ)]＝0
E[X(0)·vT(τ)]＝0 (6)；
对非线性系统先进行线性化后，得到：
Figure BDA0003921007000000051
δZ(t)＝H(t)δX(t)+v(t) (7)；
再对其进行离散，得到：
Figure BDA0003921007000000052
δZk＝HkδXk+Vk (8)；
当T为小量时，有：
Figure BDA0003921007000000053
Figure BDA0003921007000000054
至此，得到离散型非线性广义卡尔曼滤波方程如下：
Figure BDA0003921007000000061
对于观测量Z，在滤波系统中的计算如下：
Figure BDA0003921007000000062
基于上述过程对目标模块的位置和姿态运动进行观测。目标的位置运动满足牛顿第二定律，以位置、速度和加速度为状态量，组成状态矩阵：
XR＝[R V a]T (11)；
假设在观测过程中，目标为常值机动目标，即加速度不发生变化，则对应的状态模型可以写为：
Figure BDA0003921007000000063
其中03代表三阶全零矩阵，E3代表三阶单位矩阵，状态矩阵为：
Figure BDA0003921007000000064
通过视觉测量仅能观测到目标的位置信息，观测矩阵为：
HR＝[E3 03 03] (14)；
目标的姿态运动中，以姿态角、角速度和角加速度为状态量：
XA＝[Θ ω α]T (15)；
与位置运动不同的是，在刚体的姿态描述中，姿态角的一阶时间导数
Figure BDA0003921007000000065
并不等于角速度ω，他们之间存在的运动学关系通过基于3-1-2旋转方式定义的欧拉角
Figure BDA0003921007000000071
定义，则可以描述为：
Figure BDA0003921007000000072
将其改写为：
Figure BDA0003921007000000073
假设在观测过程中目标角加速度不发生变化，则对应的状态模型可以写为：
Figure BDA0003921007000000074
姿态运动的状态矩阵为：
Figure BDA0003921007000000075
通过视觉测量仅能观测到目标的姿态信息，观测矩阵为：
HA＝[E3 03 03] (20)；
S22.在进行EKF滤波估计时，考虑到在图像处理中存在诸多不确定性，可能导致质心位置、姿态角度观测的失效，从而导致观测结果发生突变，为了避免因为输入的观测结果突变导致的滤波偏差，对观测结果进行判断，一旦当前的目标位姿观测结果与上一时刻之间存在较大偏差，认为当前观测结果置信度不高，需要进行处理，对于没有得到有效位姿信息，即观测结果被置零的情况，将上一时刻的观测结果作为当前观测结果，输入到滤波过程中，对于当前位子观测信息与上一时刻之间存在较大差异的情况，则取上一时刻和当前时刻观测结果的平均值，削弱当前观测信息的突变幅度；
S23.为了描述未来目标的运动状态，即对目标运动进行预测，需要在运动信息中加入时间参数，将目标的运动信息从三维位置坐标扩展为“位置+时间”的四维运动信息，经过运动轨迹的拟合，可以预测在未来某一时刻下，目标所能到达的位置，考虑到目标在无自主机动的情况下，其运动基本满足二次函数，采用运动函数拟合的方式，在仿真中不断循环迭代，优化目标运动的拟合参数，依据二次函数公式进行推导，能够实现运动轨迹的拟合；
二次函数写为：
x＝At2/2+Bt+C (21)；
将滤波估计所能得到的位置R速度V和加速度a带入，有：
Figure BDA0003921007000000081
优选地，所述步骤S3中实现对目标模块快速捕获的方法包括以下步骤：
S31.基于空间机器人动力学模型，搭建其轨迹规划系统强化学习训练仿真环境，确定其状态量和动作量，设计奖励函数；
空间机器人的动力学模型可以写为以下形式：
Figure BDA0003921007000000082
其中
Figure BDA0003921007000000083
是对应空间机器人基座和各关节的控制力/力矩，
Figure BDA0003921007000000084
是对应空间机器人基座和各关节的位置/角度向量，
Figure BDA0003921007000000085
是对应q的速度和加速度向量，M(q)是系统的质量矩阵，
Figure BDA0003921007000000086
是与速度相关的非线性矩阵；
根据空间机器人动力学模型，选取系统状态量
Figure BDA0003921007000000087
其中Δpe为机器人末端点与期望位置之间的差，flag为机械臂末端点是否到达期望位置的标志，选取系统动作量a＝τ，在训练中，将目标位置设置为终点，机器人运动规划能否成功取决于机器人末端能否到达终点并保持稳定，因此机器人末端点与目标点之间的距离是建立奖励模型的重要参数，为了使机器人能够快速到达目标点，设置任务规划过程中的单步奖励值函数为r＝r1+r2，其中：
r1＝-ω||Δpe|| (24)；
ω为常值权重，当机械臂末端点到达目标位置时，每次给机械臂+1的常值奖励，即：
Figure BDA0003921007000000091
S32.设计基于深度确定性策略梯度算法的训练网络，DDPG算法采用Actor-Critic模式，利用Actor做出决策、Critic做出评价：首先从环境中观测得到状态s，传递给Actor根据这一状态凭现有策略做出决定得到动作a，将动作作用于环境之后，环境会给出当前步的奖励反馈r和新的状态，根据奖励反馈r，Critic将会更新对Actor的行为评价网络，Actor再沿着Critic建议的方向更新自身的策略网络，如此完成了一步训练，然后继续循环直到训练成功，由于神经网络要求训练数据之间是独立的，而Actor通过不断与环境交互得到的数据存在一定的相关性，DDPG采用了经验池的设计，将与环境交互得到的数据样本存储在经验池中，然后再从中随机选择一组数据进行训练，由此打破了数据间的关联，实现样本的独立；
S33.设计网络中的学习率、折扣因子、经验池大小、训练回合、最大迭代步等超参数；
S34.完成基于DDPG算法的空间机器人轨迹规划策略训练。
优选地，所述步骤S34中完成基于DDPG算法的空间机器人轨迹规划策略训练的方法具体包括以下步骤：
S341.Actor根据当前状态st，结合当前策略μ选择动作at＝μ(st|θμ)，并传递给环境执行该动作；
S342.环境执行动作at，转换到新的状态st+1，并产生执行动作后的奖励值rt，返回给Actor；
S343.Actor把组合(st,at,rt,st+1)存入经验池M中，作为训练网络的数据集；
S344.从经验池中随机抽取N组数据，将其作为训练现实策略网络和现实Q网络的mini-batch数据；
S345.目标策略网络根据(si,ai,ri,si+1)中的状态si+1，结合网络策略θμ′产生动作μ′(si+1)并传入目标Q网络中以便计算目标Q值，目标Q网络根据(si,ai,ri,si+1)以及si+1对应的μ′(si+1)计算目标Q值：
yi＝ri+γQ′(si+1,μ′(si+1|θμ′)|θQ′) (26)；
同时现实策略网络结合(si,ai,ri,si+1)中的si产生动作ai，传入现实Q网络中计算相应的Q值Q(si,ai|θQ)，由此定义Q网络的Loss函数，记为：
Figure BDA0003921007000000101
可以求得L针对于θQ的梯度
Figure BDA0003921007000000102
S346.优化更新现实Q网络的网络参数θQ；
S347.计算策略网络的梯度
Figure BDA0003921007000000103
从上式可以看出，策略网络的梯度更新公式由N个两部分相乘的和组成，前半部分通过Critic得到，用于评价Actor要如何移动才能得到更大的Q值；后半部分由Actor得到，用于体现Actor要如何更新自身的参数才能使策略做出能够得到更大Q值的动作，总体来说，就是要让策略网络学会向获得更大Q值的方向更新自身参数；
S348.优化更新现实策略网络的网络参数θμ；
S349.分别对目标策略网络和目标Q网络的网络参数θμ′和θQ′进行软更新：
θQ′←ηθQ+(1-η)θQ′ (29)；
θμ′←ηθμ+(1-η)θμ′ (30)；
当完成以上步骤时，就完成了一个时间步内的训练，从步骤S341重新开始循环则开始了下一个时间步的训练，当完成了所有时间步的训练后，即为完成了一个完整回合的训练，通过回合的不停更迭则会使网络参数向最大化Q值的方向更新发展。
综上所述，由于采用了上述技术方案，本发明的有益效果是：
1、本申请中，基于模拟双目视觉相机和拟合平面法解算目标模块位姿信息的方法可以在仿真环境下模拟双目视觉相机的成像，进而按照拟合平面法解算目标模块的位置和姿态信息，改进了常用的直线端点检测目标顶点时可能出现的图像特征过少、目标位姿信息误差较大等问题。
2、本申请中，基于EKF的估计方法能够根据多帧连续观测的目标图像，校正目标的运动状态，得到目标运动的滤波估计结果，进而根据二次函数拟合无自主机动目标的运动。
3、本申请中，基于强化学习的空间机器人轨迹规划方法，以空间机器人的动力学模型为训练环境，保证了训练结果向真实空间机器人系统的可迁移性；所设计的奖励函数与末端点和期望位置之间的距离相关，能够引导机器人末端点尽快向期望位置靠近，有效地提高了空间机器人轨迹规划系统的训练效果。
附图说明
图1为本发明提出的基于模拟双目视觉测量的空间机器人在轨捕获方法流程图；
图2为本发明提出的相机可视平面筛选方法示意图；
图3为本发明提出的平面指外法向量与特征点间矢量关系示意图；
图4为本发明提出的目标模块运动信息求解流程图；
图5为本发明提出的立方体目标模块示意图；
图6为本发明提出的滤波估计位置和速度误差；
图7为本发明提出的滤波估计姿态角和角速度误差；
图8为本发明提出的基于二次函数参数拟合的位置运动预测；
图9为本发明提出的训练过程的总奖励值及所用步数分布情况；
图10为本发明提出的训练过程中的成功率。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
请参阅图1-10，本发明提供一种技术方案：
一种基于模拟双目视觉测量的空间机器人在轨捕获方法，包括以下步骤：
S1.基于模拟双目视觉相机获取目标模块的特征点信息，通过特征点匹配算法获取特征点的三维位置标记，进而按照拟合平面法解算目标模块的位置和姿态信息；
步骤S1中基于模拟双目视觉相机获取目标模块的特征点信息的方法包括以下步骤：
S11.在仿真环境下确定立方体目标模块的尺寸、颜色、指外法向量参数信息，如图5所示，作为后续模拟双目视觉相机的观测对象；
S12.将步骤S11中的立方体目标模块载入，由于目标在相机中的投影并非平行投影，而是汇聚于相机光心，因此在对目标进行拍摄图像模拟时，需要将惯性空间中，目标各顶点坐标转换为图像中的像素位置，还需要对图像中可以看到的平面进行判断和筛选，最终在图像中保留真正可以看到的平面；
步骤S12的具体实现方法包括以下步骤：
S121.求解相机坐标系和像素坐标系下立方体目标上全部顶点的坐标，记为Xfc和Xfuv；
S122.进行第一次图像可视筛选，计算目标每个表面的指外法向量，指外法向量朝向相机一侧，即指外法向量和相机坐标系的Z轴的夹角大于90°，该平面在相机图像中才有可能可以看到，在平行投影的情况下，所有nout·zc＞0的平面都能够显示在相机图像中，其中nout为平面的指外法向量，zc为相机坐标系的Z轴方向，但由于相机采用透视成像，物体在投影时视野盲区变大，这使得相机图像中可以看到的平面的约束增强了，因此需要对进行第二次筛选；
S123.进行第二次图像可视筛选，确定在相机图像中真正可以看到的平面，将每个面的顶点按一定的顺序编号，使得顶点螺旋指向目标的外部，在像素平面上将这些顶点顺次连线，螺旋朝内则该平面能够在相机图像中看见，螺旋朝外则看到的是该平面的背面，对于实体目标，该平面在物体内部是看不见的，如图2(b)所示。
S13.采用最小特征值算法检测图像的角点，该方法的理论原理为Harr i s角点检测算法，在图像中能够检测到边缘、拐角等特征位置的像素点，相比Hough直线检测，角点检测能够获取更多数目的特征点，基于最小特征值算法的特征点识别结果；
S14.采用金字塔光流法进行左右相机视图特征点的匹配，求解所有匹配后的特征点对的三维空间坐标；
S15.对每一对匹配成功的点对，在各自的图像中进行颜色标记。以特征点为中心，选取一定像素范围，通过像素的RGB值进行判断，标记特征点附近像素点的颜色，将特征点附近出现过的颜色标记为该特征点的颜色，根据立方体目标的几何特征，一个特征点具有至少一个，至多三个颜色标记；
S16.依据带有颜色标签的特征点，寻找目标表面所在的平面，继而进行立方体目标的三维状态还原；
步骤S16的实现方法具体包括以下步骤：
S161.定义某一个颜色平面的方程为：
ax+by+cz+d＝0 (1)；
提取具有相同颜色标签，对这些特征点所在平面进行线性拟合，拟合得到的平面函数为：
f(z)＝p00+p10 x+p01 y (2)；
其中三个系数与平面方程的四个参数的关系为：
[a b c d]＝[p10 p01 -1 p00] (3)；
S162.选取图像中检测到的不属于该平面的一个特征点，再从属于拟合平面的特征点中任选一个，建立从平面指向目标其他面上特征点的矢量n1，令所拟合平面的指外法向量为n0，两矢量之间的夹角为a0,当平面的法向量指向平面外，则a0必然为一钝角,如图3所示，当求解的平面法向量与特征点矢量之间夹角为钝角时，该平面参数是拟合平面的指外法向量，对不成钝角的情况则将平面方程前三个参数取负值；
S163.当得到多个平面的指外法向量后，将目标模块的质心位置和姿态角作为变量，设计指标函数并采用优化方法进行求解。指标函数写为如下形式：
Figure BDA0003921007000000141
其中rcenter代表惯性空间立方体质心位置矢量，
Figure BDA0003921007000000142
代表第i个特征点惯性空间坐标，n代表特征点的总数，L表示目标模块边长，
Figure BDA0003921007000000143
表示拟合的平面依据立方体边长平移后的过质心平面的法向量，
Figure BDA0003921007000000144
表示第k个拟合平面的指外法向量，m表示拟合的平面个数，AΘ表示从姿态角Θ时惯性系与本体系的3-1-2坐标转换矩阵，
Figure BDA0003921007000000145
则表示零姿态下对应颜色平面的指外法向量；
指标函数的前两项与质心位置有关，其中第一项代表质心位置到特征点的距离之和，在立方体中，立方体中心到各个棱边上的点的距离之和是最小的；第二项则是质心到过质心的拟合平面的距离之和，这一距离和的理想值为零；指标函数的第三项与立方体姿态有关，将零姿态下的平面指外法向量经过姿态坐标转换，应当与拟合出的平面指外法向量重合，因此将平面法向量之间的误差之和作为指标，误差越小说明姿态角求解越准确。
S164.对目标函数进行优化求解，得到目标模块质心位置和三轴姿态；
S2.根据步骤S1的目标识别过程，对目标模块进行持续观测和跟踪，基于EKF方法对目标的运动状态进行求解，得到目标运动的滤波估计结果，如图4所示，进而拟合目标模块的运动轨迹，实现对目标的运动预测；
步骤S2中实现对目标的运动预测的方法包括以下步骤：
S21.扩展卡尔曼滤波将非线性系统中的非线性函数f(*)和h(*)围绕展开为泰勒级数，保留第一项，略去其他项，得到一个近似的一阶线性化模型，然后按照标准卡尔曼滤波的流程进行滤波估计，一般的非线性系统可以由以下方程描述；
Figure BDA0003921007000000151
Z(t)＝h[X(t),t]+v(t)； (5)；
式中W(t)和v(t)均是彼此不相关的零均值白噪声序列，他们与初始状态X(0)或X0也不相关，即对于t>t0有其统计学特性如下：
E[w(t)]＝0，E[w(t)·wT(τ)]＝q(t)δ(t-τ)
E[v(t)]＝0，E[v(t)·vT(τ)]＝r(t)δ(t-τ)
E[w(t)·vT(τ)]＝0，E[X(0)·wT(τ)]＝0
E[X(0)·vT(τ)]＝0 (6)；
对非线性系统先进行线性化后，得到：
Figure BDA0003921007000000152
δZ(t)＝H(t)δX(t)+v(t) (7)；
再对其进行离散，得到：
Figure BDA0003921007000000161
δZk＝HkδXk+Vk (8)；
当T为小量时，有：
Figure BDA0003921007000000162
Figure BDA0003921007000000163
至此，得到离散型非线性广义卡尔曼滤波方程如下：
Figure BDA0003921007000000164
对于观测量Z，在滤波系统中的计算如下：
Figure BDA0003921007000000165
基于上述过程对目标模块的位置和姿态运动进行观测。目标的位置运动满足牛顿第二定律，以位置、速度和加速度为状态量，组成状态矩阵：
XR＝[R V a]T (11)；
假设在观测过程中，目标为常值机动目标，即加速度不发生变化，则对应的状态模型可以写为：
Figure BDA0003921007000000171
其中03代表三阶全零矩阵，E3代表三阶单位矩阵。状态矩阵为：
Figure BDA0003921007000000172
通过视觉测量仅能观测到目标的位置信息，观测矩阵为：
HR＝[E3 03 03] (14)；
目标的姿态运动中，以姿态角、角速度和角加速度为状态量：
XA＝[Θ ω α]T (15)；
与位置运动不同的是，在刚体的姿态描述中，姿态角的一阶时间导数
Figure BDA0003921007000000173
并不等于角速度ω，他们之间存在的运动学关系通过基于3-1-2旋转方式定义的欧拉角
Figure BDA0003921007000000174
定义，则可以描述为：
Figure BDA0003921007000000175
将其改写为：
Figure BDA0003921007000000176
假设在观测过程中目标角加速度不发生变化，则对应的状态模型可以写为：
Figure BDA0003921007000000181
姿态运动的状态矩阵为：
Figure BDA0003921007000000182
通过视觉测量仅能观测到目标的姿态信息，观测矩阵为：
HA＝[E3 03 03] (20)；
S22.在进行EKF滤波估计时，考虑到在图像处理中存在诸多不确定性，可能导致质心位置、姿态角度观测的失效，从而导致观测结果发生突变，为了避免因为输入的观测结果突变导致的滤波偏差，对观测结果进行判断，一旦当前的目标位姿观测结果与上一时刻之间存在较大偏差，认为当前观测结果置信度不高，需要进行处理，对于没有得到有效位姿信息，即观测结果被置零的情况，将上一时刻的观测结果作为当前观测结果，输入到滤波过程中，对于当前位子观测信息与上一时刻之间存在较大差异的情况，则取上一时刻和当前时刻观测结果的平均值，削弱当前观测信息的突变幅度；
基于前文描述的滤波估计方法，设计EKF参数，其中环境噪声为5×10-2N，位置观测噪声估计值为0.1m，姿态观测噪声估计值0.1deg，此外，双目相机的外部参数矩阵为：
Figure BDA0003921007000000183
内部参数矩阵分别为：
Figure BDA0003921007000000184
最终得到对立方体目标的滤波估计位置和速度误差如图6所示，姿态角和角速度误差如图7所示，经过滤波估计，目标位置的估计误差可以收敛至0.05m以内，相比直接观测结果噪声减弱，误差缩小，目标运动速度的估计结果则更加贴近真实状态，经过约5秒的观测，运动速度的估计误差能够收敛到±0.02m/s以内，运动观测精度较高，姿态角的估计误差可以从观测结果的±2deg以内进一步收敛，在大部分情况下能够达到±1deg，姿态角速度的估计误差则能够保持在±2deg/s以内，同样具有较高的估计精度。
S23.为了描述未来目标的运动状态，即对目标运动进行预测，需要在运动信息中加入时间参数，将目标的运动信息从三维位置坐标扩展为“位置+时间”的四维运动信息，经过运动轨迹的拟合，可以预测在未来某一时刻下，目标所能到达的位置，考虑到目标在无自主机动的情况下，其运动基本满足二次函数，采用运动函数拟合的方式，在仿真中不断循环迭代，优化目标运动的拟合参数，依据二次函数公式进行推导，能够实现运动轨迹的拟合；
二次函数写为：
x＝At2/2+Bt+C (21)；
将滤波估计所能得到的位置R速度V和加速度a带入，有：
Figure BDA0003921007000000191
对目标施加幅值为[-0.10.11]N的恒定作用力和[0.010.02-0.01]Nm的恒定作用力矩，仿真步长0.05s，仿真时长5s，依据每一时刻滤波得到的目标运动状态，对运动轨迹进行迭代优化，并估计t＝5s时刻的目标位置和姿态，仿真结果如图8所示，随着仿真的进行，运动轨迹的参数不断迭代拟合，运动轨迹的拟合精度有所提升，运动预测的准确度也相应提升，图8的(a)展示了每一个仿真步长之内对t＝5s时刻目标到达位置的预测，图8的(b)展示了从第三秒到第五秒每一个仿真步长之内对第五秒目标到达位置的预测，随着仿真的进行，对目标位置的预测逐渐靠近真实结果；
S3.基于强化学习方法训练得到空间机器人的轨迹规划系统，将预测的目标位姿信息输入空间机器人的轨迹规划系统，实现对目标模块的快速捕获；
步骤S3中实现对目标模块快速捕获的方法包括以下步骤：
S31.基于空间机器人动力学模型，搭建其轨迹规划系统强化学习训练仿真环境，确定其状态量和动作量，设计奖励函数；
空间机器人的动力学模型可以写为以下形式：
Figure BDA0003921007000000201
其中
Figure BDA0003921007000000202
是对应空间机器人基座和各关节的控制力/力矩，
Figure BDA0003921007000000203
是对应空间机器人基座和各关节的位置/角度向量，
Figure BDA0003921007000000204
是对应q的速度和加速度向量，M(q)是系统的质量矩阵，
Figure BDA0003921007000000205
是与速度相关的非线性矩阵；
根据空间机器人动力学模型，选取系统状态量
Figure BDA0003921007000000206
其中Δpe为机器人末端点与期望位置之间的差，flag为机械臂末端点是否到达期望位置的标志，选取系统动作量a＝τ，在训练中，将目标位置设置为终点，机器人运动规划能否成功取决于机器人末端能否到达终点并保持稳定，因此机器人末端点与目标点之间的距离是建立奖励模型的重要参数，为了使机器人能够快速到达目标点，设置任务规划过程中的单步奖励值函数为r＝r1+r2，其中：
r1＝-ω||Δpe|| (24)；
ω为常值权重，当机械臂末端点到达目标位置时，每次给机械臂+1的常值奖励，即：
Figure BDA0003921007000000207
S32.设计基于深度确定性策略梯度算法的训练网络，DDPG算法采用Actor-Critic模式，利用Actor做出决策、Critic做出评价：首先从环境中观测得到状态s，传递给Actor根据这一状态凭现有策略做出决定得到动作a，将动作作用于环境之后，环境会给出当前步的奖励反馈r和新的状态，根据奖励反馈r，Critic将会更新对Actor的行为评价网络，Actor再沿着Critic建议的方向更新自身的策略网络，如此完成了一步训练，然后继续循环直到训练成功，由于神经网络要求训练数据之间是独立的，而Actor通过不断与环境交互得到的数据存在一定的相关性，DDPG采用了经验池的设计，将与环境交互得到的数据样本存储在经验池中，然后再从中随机选择一组数据进行训练，由此打破了数据间的关联，实现样本的独立；
S33.设计网络中的学习率、折扣因子、经验池大小、训练回合、最大迭代步等超参数；
S34.完成基于DDPG算法的空间机器人轨迹规划策略训练；
步骤S34中完成基于DDPG算法的空间机器人轨迹规划策略训练的方法具体包括以下步骤：
S341.Actor根据当前状态st，结合当前策略μ选择动作at＝μ(st|θμ)，并传递给环境执行该动作；
S342.环境执行动作at，转换到新的状态st+1，并产生执行动作后的奖励值rt，返回给Actor；
S343.Actor把组合(st,at,rt,st+1)存入经验池M中，作为训练网络的数据集；
S344.从经验池中随机抽取N组数据，将其作为训练现实策略网络和现实Q网络的mini-batch数据；
S345.目标策略网络根据(si,ai,ri,si+1)中的状态si+1，结合网络策略θμ′产生动作μ′(si+1)并传入目标Q网络中以便计算目标Q值，目标Q网络根据(si,ai,ri,si+1)以及si+1对应的μ′(si+1)计算目标Q值：
yi＝ri+γQ′(si+1,μ′(si+1|θμ′)|θQ′) (26)；
同时现实策略网络结合(si,ai,ri,si+1)中的si产生动作ai，传入现实Q网络中计算相应的Q值Q(si,ai|θQ)。由此定义Q网络的Loss函数，记为：
Figure BDA0003921007000000221
可以求得L针对于θQ的梯度
Figure BDA0003921007000000222
S346.优化更新现实Q网络的网络参数θQ；
S347.计算策略网络的梯度
Figure BDA0003921007000000223
从上式可以看出，策略网络的梯度更新公式由N个两部分相乘的和组成，前半部分通过Critic得到，用于评价Actor要如何移动才能得到更大的Q值；后半部分由Actor得到，用于体现Actor要如何更新自身的参数才能使策略做出能够得到更大Q值的动作，总体来说，就是要让策略网络学会向获得更大Q值的方向更新自身参数；
S348.优化更新现实策略网络的网络参数θμ；
S349.分别对目标策略网络和目标Q网络的网络参数θμ′和θQ′进行软更新：
θQ′←ηθQ+(1-η)θQ′ (29)；
θμ′←ηθμ+(1-η)θμ′ (30)；
当完成以上步骤时，就完成了一个时间步内的训练，从步骤S341重新开始循环则开始了下一个时间步的训练，当完成了所有时间步的训练后，即为完成了一个完整回合的训练，通过回合的不停更迭则会使网络参数向最大化Q值的方向更新发展；
假设空间机器人基座位姿可控，将空间机器人模型简化为平面三自由度机械臂。基于空间机器人动力学模型，搭建其轨迹规划系统强化学习训练仿真环境，确定其状态量和动作量，设计奖励函数；
设计基于DDPG算法的训练网络，策略网络和Q网络均采用三层神经网络的结构设计，策略网络的输入为系统状态s，输出为采取的动作a，策略网络输出层采用tanh激活函数，使输出的连续动作限制在[-1,1]范围内，再根据需求乘以动作幅值参数得到期望动作输出，而其余层均采用ReLU激活函数，Q网络的输入为系统状态s、动作a以及一个一维的偏置单元b，输出为该状态-动作对的评分也即Q值，Q网络中均采用ReLU激活函数；
设计网络中的学习率、折扣因子、经验池大小、训练回合、最大迭代步等超参数，策略网络Q网络的学习率均设为0.001，折扣因子设置为0.9，设置其经验池M的大小为10000组数据，当经验池中数据存满之后开始从中随机抽取64组数据训练策略网络和Q网络，训练运行最大回合数设置为3000次，同时为避免过长的无意义的训练情况出现，设置每个回合的最大迭代步为200次，当超过200次仍未成功到达目标点时则强制结束当前回合，并进入下一回合的训练；
基于DDPG算法完成空间机器人轨迹规划策略训练，经过3000次训练，得到整个过程每一回合中得到的总奖励值及所用迭代总步数如图9所示，从图中可以看出，训练的初始时刻(约1-250回合)机器人处于探索过程，每一回合的得分均不理想，在经过约50回合之后系统获得的总奖励值虽仍处于较低水平但有一定改善和提升，这是因为所设计的经验池M的大小为10000，在前50回合中，经验池还未存满，每一次都是随机选取动作进行探索，当经验池存满之后开始从中随机选取数据训练策略网络和Q网络，此时机器人动作是由经过一定训练的策略网络产生，因此训练效果能够有一定程度的提升，同时前250回合左右的训练均为经过200次迭代后强制结束，随后才逐渐实现训练过程的收敛，随着训练进行，机器人经过40-65步能够实现收敛，且每个回合的总奖励值主要分布在29-30左右，这与所设计的奖励函数及回合结束条件有关，为了使机器人末端点到达期望位置后仍能保持，设置机器人末端点在目标位置保持30个步长后回合结束，且机器人末端点每次经过目标位置均能获得+1的奖励值，因此总奖励值集中分布在29-30则反映了经过训练的机械臂能够快速收敛至目标位置，并保持位置基本不变；
图9反映了训练过程中获得奖励和所用步数的分布情况，但对训练过程变化趋势的描述并不明显，因此需要对数据进一步处理，为在一定程度上避免随机情况的影响，采用若干回合总奖励值和所用总步数的平均值作为一个阶段的平均奖励值和平均步数，这里以每10回合的平均情况进行说明，得到平均奖励值和平均步数的变化曲线，由此可知，训练过程大致可以分为4个阶段：训练初始的第1阶段奖励值约为-37，迭代步数为200次，对应的状态为机械臂随机选取动作对环境进行探索；第2阶段开始于经验池存满之后，即开始对策略网络和Q网络进行训练之后，此时系统得到的奖励值立即有一个较大的提升，但每回合所需迭代步数仍为200次，对应的状态为机械臂末端点能够靠近目标位置，但仍不能到达或保持；随后进入第3阶段，此时经过训练的策略网络已逐渐能够给出理想的运动轨迹，使系统经过若干迭代步之后能够收敛于目标位置，但此时所需的步数还较高，对应的奖励值也比较大，这说明机械臂末端会不断经过目标位置，但还不能有效保持稳定；最后第4阶段系统趋于稳定，一般每个回合经过约40-60次迭代即能实现收敛，对应的奖励值也处于30左右，训练过程并非一成不变，也会存在一定程度的波动；
对训练过程中每个回合的结束情况进行记录，若该回合经200步迭代后被强制结束，则记录该回合训练不成功；若该回合在200步迭代以内就完成，则记录该回合训练成功，对每100回合的训练成功情况取均值，得到训练过程不同阶段中成功率的情况如图10所示，从图中可以看出，随着训练的进行，每个阶段的训练成功率迅速提升，且后续能够保持约95％以上的成功率；
对训练结果进行保存，当进行新任务的规划时，只需按照上述训练完成得到的参数进行轨迹规划即可。
综上所述，本实施例所提供的一种基于模拟双目视觉测量的空间机器人在轨捕获方法，在仿真环境中模拟双目视觉相机，将目标模块抽象为立方体，通过模拟双目CCD摄像机来模拟人类的视觉系统，同时在不同位置拍摄目标模块的左右图像，利用立体匹配算法对左右图像对进行处理，获得目标模块的视差图，结合投影几何成像以及三角测量等光学原理求取物体的深度信息，达到目标位姿信息测量的效果；然后通过对目标进行持续观测和跟踪，估计目标真实的运动状态，进而预测目标在未来某一时刻的运动情况；基于预测结果，设计空间机器人的轨迹规划系统，规划机器人从当前状态到预计目标位置的运动轨迹，最终实现对目标模块的在轨捕获。
实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (7)

1.一种基于模拟双目视觉测量的空间机器人在轨捕获方法，其特征在于，包括以下步骤：
S1.基于模拟双目视觉相机获取目标模块的特征点信息，通过特征点匹配算法获取特征点的三维位置标记，进而按照拟合平面法解算目标模块的位置和姿态信息；
S2.根据步骤S1的目标识别过程，对目标模块进行持续观测和跟踪，基于EKF方法对目标的运动状态进行求解，得到目标运动的滤波估计结果，进而拟合目标模块的运动轨迹，实现对目标的运动预测；
S3.基于强化学习方法训练得到空间机器人的轨迹规划系统，将预测的目标位姿信息输入空间机器人的轨迹规划系统，实现对目标模块的快速捕获。
2.根据权利要求1所述的一种基于模拟双目视觉测量的空间机器人在轨捕获方法，其特征在于，所述步骤S1中基于模拟双目视觉相机获取目标模块的特征点信息的方法包括以下步骤：
S11.在仿真环境下确定立方体目标模块的尺寸、颜色、指外法向量参数信息，作为后续模拟双目视觉相机的观测对象；
S12.将步骤S11中的立方体目标模块载入，由于目标在相机中的投影并非平行投影，而是汇聚于相机光心，因此在对目标进行拍摄图像模拟时，需要将惯性空间中，目标各顶点坐标转换为图像中的像素位置，还需要对图像中可以看到的平面进行判断和筛选，最终在图像中保留真正可以看到的平面。
S13.采用最小特征值算法检测图像的角点，在图像中能够检测到边缘、拐角特征位置的像素点；
S14.采用金字塔光流法进行左右相机视图特征点的匹配，求解所有匹配后的特征点对的三维空间坐标；
S15.对每一对匹配成功的点对，在各自的图像中进行颜色标记,以特征点为中心，选取一定像素范围，通过像素的RGB值进行判断，标记特征点附近像素点的颜色，将特征点附近出现过的颜色标记为该特征点的颜色，根据立方体目标的几何特征，一个特征点具有至少一个，至多三个颜色标记；
S16.依据带有颜色标签的特征点，寻找目标表面所在的平面，继而进行立方体目标的三维状态还原。
3.根据权利要求2所述的一种基于模拟双目视觉测量的空间机器人在轨捕获方法，其特征在于，所述步骤S12的具体实现方法包括以下步骤：
S121.求解相机坐标系和像素坐标系下立方体目标上全部顶点的坐标，记为Xfc和Xfuv；
S122.进行第一次图像可视筛选，计算目标每个表面的指外法向量，指外法向量朝向相机一侧，即指外法向量和相机坐标系的Z轴的夹角大于90°，该平面在相机图像中才有可能可以看到，在平行投影的情况下，所有nout·zc＞0的平面都能够显示在相机图像中，其中nout为平面的指外法向量，zc为相机坐标系的Z轴方向，但由于相机采用透视成像，物体在投影时视野盲区变大，这使得相机图像中可以看到的平面的约束增强了，因此需要对进行第二次筛选；
S123.进行第二次图像可视筛选，确定在相机图像中真正可以看到的平面，将每个面的顶点按一定的顺序编号，使得顶点螺旋指向目标的外部，在像素平面上将这些顶点顺次连线，螺旋朝内则该平面能够在相机图像中看见，螺旋朝外则看到的是该平面的背面，对于实体目标，该平面在物体内部是看不见的。
4.根据权利要求2所述的一种基于模拟双目视觉测量的空间机器人在轨捕获方法，其特征在于，所述步骤S16的实现方法具体包括以下步骤：
S161.定义某一个颜色平面的方程为：
ax+by+cz+d＝0 (1)；
提取具有相同颜色标签，对这些特征点所在平面进行线性拟合，拟合得到的平面函数为：
f(z)＝p00+p10 x+p01 y (2)；
其中三个系数与平面方程的四个参数的关系为：
[a b c d]＝[p10 p01 -1 p00] (3)；
S162.选取图像中检测到的不属于该平面的一个特征点，再从属于拟合平面的特征点中任选一个，建立从平面指向目标其他面上特征点的矢量n1，令所拟合平面的指外法向量为n0，两矢量之间的夹角为a0,当平面的法向量指向平面外，则a0必然为一钝角,当求解的平面法向量与特征点矢量之间夹角为钝角时，该平面参数是拟合平面的指外法向量，对不成钝角的情况则将平面方程前三个参数取负值；
S163.当得到多个平面的指外法向量后，将目标模块的质心位置和姿态角作为变量，设计指标函数并采用优化方法进行求解。指标函数写为如下形式：
Figure FDA0003921006990000031
其中rcenter代表惯性空间立方体质心位置矢量，
Figure FDA0003921006990000032
代表第i个特征点惯性空间坐标，n代表特征点的总数，L表示目标模块边长，
Figure FDA0003921006990000033
表示拟合的平面依据立方体边长平移后的过质心平面的法向量，
Figure FDA0003921006990000034
表示第k个拟合平面的指外法向量，m表示拟合的平面个数，AΘ表示从姿态角Θ时惯性系与本体系的3-1-2坐标转换矩阵，
Figure FDA0003921006990000035
则表示零姿态下对应颜色平面的指外法向量；
S164.对目标函数进行优化求解，得到目标模块质心位置和三轴姿态。
5.根据权利要求1所述的一种基于模拟双目视觉测量的空间机器人在轨捕获方法，其特征在于，所述步骤S2中实现对目标的运动预测的方法包括以下步骤：
S21.扩展卡尔曼滤波将非线性系统中的非线性函数f(*)和h(*)围绕展开为泰勒级数，保留第一项，略去其他项，得到一个近似的一阶线性化模型，然后按照标准卡尔曼滤波的流程进行滤波估计，一般的非线性系统可以由以下方程描述；
Figure FDA0003921006990000041
Z(t)＝h[X(t),t]+v(t)； (5)；
式中W(t)和v(t)均是彼此不相关的零均值白噪声序列，他们与初始状态X(0)或X0也不相关，即对于t>t0有其统计学特性如下：
E[w(t)]＝0，E[w(t)·wT(τ)]＝q(t)δ(t-τ)
E[v(t)]＝0，E[v(t)·vT(τ)]＝r(t)δ(t-τ)
E[w(t)·vT(τ)]＝0，E[X(0)·wT(τ)]＝0
E[X(0)·vT(τ)]＝0 (6)；
对非线性系统先进行线性化后，得到：
Figure FDA0003921006990000042
δZ(t)＝H(t)δX(t)+v(t) (7)；
再对其进行离散，得到：
Figure FDA0003921006990000043
δZk＝HkδXk+Vk (8)；
当T为小量时，有：
Figure FDA0003921006990000044
Figure FDA0003921006990000051
至此，得到离散型非线性广义卡尔曼滤波方程如下：
Figure FDA0003921006990000052
对于观测量Z，在滤波系统中的计算如下：
Figure FDA0003921006990000053
基于上述过程对目标模块的位置和姿态运动进行观测。目标的位置运动满足牛顿第二定律，以位置、速度和加速度为状态量，组成状态矩阵：
XR＝[R V a]T (11)；
假设在观测过程中，目标为常值机动目标，即加速度不发生变化，则对应的状态模型可以写为：
Figure FDA0003921006990000054
其中03代表三阶全零矩阵，E3代表三阶单位矩阵，状态矩阵为：
Figure FDA0003921006990000061
通过视觉测量仅能观测到目标的位置信息，观测矩阵为：
HR＝[E3 03 03] (14)；
目标的姿态运动中，以姿态角、角速度和角加速度为状态量：
XA＝[Θ ω α]T (15)；
与位置运动不同的是，在刚体的姿态描述中，姿态角的一阶时间导数
Figure FDA0003921006990000062
并不等于角速度ω，他们之间存在的运动学关系通过基于3-1-2旋转方式定义的欧拉角
Figure FDA0003921006990000063
定义，则可以描述为：
Figure FDA0003921006990000064
将其改写为：
Figure FDA0003921006990000065
假设在观测过程中目标角加速度不发生变化，则对应的状态模型可以写为：
Figure FDA0003921006990000066
姿态运动的状态矩阵为：
Figure FDA0003921006990000067
通过视觉测量仅能观测到目标的姿态信息，观测矩阵为：
HA＝[E3 03 03] (20)；
S22.在进行EKF滤波估计时，考虑到在图像处理中存在诸多不确定性，可能导致质心位置、姿态角度观测的失效，从而导致观测结果发生突变，为了避免因为输入的观测结果突变导致的滤波偏差，对观测结果进行判断，一旦当前的目标位姿观测结果与上一时刻之间存在较大偏差，认为当前观测结果置信度不高，需要进行处理，对于没有得到有效位姿信息，即观测结果被置零的情况，将上一时刻的观测结果作为当前观测结果，输入到滤波过程中，对于当前位子观测信息与上一时刻之间存在较大差异的情况，则取上一时刻和当前时刻观测结果的平均值，削弱当前观测信息的突变幅度；
S23.为了描述未来目标的运动状态，即对目标运动进行预测，需要在运动信息中加入时间参数，将目标的运动信息从三维位置坐标扩展为“位置+时间”的四维运动信息，经过运动轨迹的拟合，可以预测在未来某一时刻下，目标所能到达的位置，考虑到目标在无自主机动的情况下，其运动基本满足二次函数，采用运动函数拟合的方式，在仿真中不断循环迭代，优化目标运动的拟合参数，依据二次函数公式进行推导，能够实现运动轨迹的拟合；
二次函数写为：
x＝At2/2+Bt+C (21)；
将滤波估计所能得到的位置R速度V和加速度a带入，有：
Figure FDA0003921006990000071
6.根据权利要求1所述的一种基于模拟双目视觉测量的空间机器人在轨捕获方法，其特征在于，所述步骤S3中实现对目标模块快速捕获的方法包括以下步骤：
S31.基于空间机器人动力学模型，搭建其轨迹规划系统强化学习训练仿真环境，确定其状态量和动作量，设计奖励函数；
空间机器人的动力学模型可以写为以下形式：
Figure FDA0003921006990000081
其中
Figure FDA0003921006990000082
是对应空间机器人基座和各关节的控制力/力矩，
Figure FDA0003921006990000083
是对应空间机器人基座和各关节的位置/角度向量，
Figure FDA0003921006990000084
是对应q的速度和加速度向量，M(q)是系统的质量矩阵，
Figure FDA0003921006990000085
是与速度相关的非线性矩阵；
根据空间机器人动力学模型，选取系统状态量
Figure FDA0003921006990000086
其中Δpe为机器人末端点与期望位置之间的差，flag为机械臂末端点是否到达期望位置的标志，选取系统动作量a＝τ，在训练中，将目标位置设置为终点，机器人运动规划能否成功取决于机器人末端能否到达终点并保持稳定，因此机器人末端点与目标点之间的距离是建立奖励模型的重要参数，为了使机器人能够快速到达目标点，设置任务规划过程中的单步奖励值函数为r＝r1+r2，其中：
r1＝-ω||Δpe|| (24)；
ω为常值权重，当机械臂末端点到达目标位置时，每次给机械臂+1的常值奖励，即：
Figure FDA0003921006990000087
S32.设计基于深度确定性策略梯度算法的训练网络，DDPG算法采用Actor-Critic模式，利用Actor做出决策、Critic做出评价：首先从环境中观测得到状态s，传递给Actor根据这一状态凭现有策略做出决定得到动作a，将动作作用于环境之后，环境会给出当前步的奖励反馈r和新的状态，根据奖励反馈r，Critic将会更新对Actor的行为评价网络，Actor再沿着Critic建议的方向更新自身的策略网络，如此完成了一步训练，然后继续循环直到训练成功，由于神经网络要求训练数据之间是独立的，而Actor通过不断与环境交互得到的数据存在一定的相关性，DDPG采用了经验池的设计，将与环境交互得到的数据样本存储在经验池中，然后再从中随机选择一组数据进行训练，由此打破了数据间的关联，实现样本的独立；
S33.设计网络中的学习率、折扣因子、经验池大小、训练回合、最大迭代步等超参数；
S34.完成基于DDPG算法的空间机器人轨迹规划策略训练。
7.根据权利要求6所述的一种基于模拟双目视觉测量的空间机器人在轨捕获方法，其特征在于，所述步骤S34中完成基于DDPG算法的空间机器人轨迹规划策略训练的方法具体包括以下步骤：
S341.Actor根据当前状态st，结合当前策略μ选择动作at＝μ(st|θμ)，并传递给环境执行该动作；
S342.环境执行动作at，转换到新的状态st+1，并产生执行动作后的奖励值rt，返回给Actor；
S343.Actor把组合(st,at,rt,st+1)存入经验池M中，作为训练网络的数据集；
S344.从经验池中随机抽取N组数据，将其作为训练现实策略网络和现实Q网络的mini-batch数据；
S345.目标策略网络根据(si,ai,ri,si+1)中的状态si+1，结合网络策略θμ′产生动作μ′(si+1)并传入目标Q网络中以便计算目标Q值，目标Q网络根据(si,ai,ri,si+1)以及si+1对应的μ′(si+1)计算目标Q值：
yi＝ri+γQ′(si+1,μ′(si+1|θμ′)|θQ′) (26)；
同时现实策略网络结合(si,ai,ri,si+1)中的si产生动作ai，传入现实Q网络中计算相应的Q值Q(si,ai|θQ)，由此定义Q网络的Loss函数，记为：
Figure FDA0003921006990000091
可以求得L针对于θQ的梯度
Figure FDA0003921006990000092
S346.优化更新现实Q网络的网络参数θQ；
S347.计算策略网络的梯度
Figure FDA0003921006990000101
从上式可以看出，策略网络的梯度更新公式由N个两部分相乘的和组成，前半部分通过Critic得到，用于评价Actor要如何移动才能得到更大的Q值；后半部分由Actor得到，用于体现Actor要如何更新自身的参数才能使策略做出能够得到更大Q值的动作，总体来说，就是要让策略网络学会向获得更大Q值的方向更新自身参数；
S348.优化更新现实策略网络的网络参数θμ；
S349.分别对目标策略网络和目标Q网络的网络参数θμ′和θQ′进行软更新：
θQ′←ηθQ+(1-η)θQ′ (29)；
θμ′←ηθμ+(1-η)θμ′ (30)；
当完成以上步骤时，就完成了一个时间步内的训练，从步骤S341重新开始循环则开始了下一个时间步的训练，当完成了所有时间步的训练后，即为完成了一个完整回合的训练，通过回合的不停更迭则会使网络参数向最大化Q值的方向更新发展。
CN202211355555.9A 2022-11-01 2022-11-01 一种基于模拟双目视觉测量的空间机器人在轨捕获方法 Pending CN115619828A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211355555.9A CN115619828A (zh) 2022-11-01 2022-11-01 一种基于模拟双目视觉测量的空间机器人在轨捕获方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211355555.9A CN115619828A (zh) 2022-11-01 2022-11-01 一种基于模拟双目视觉测量的空间机器人在轨捕获方法

Publications (1)

Publication Number Publication Date
CN115619828A true CN115619828A (zh) 2023-01-17

Family

ID=84876510

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211355555.9A Pending CN115619828A (zh) 2022-11-01 2022-11-01 一种基于模拟双目视觉测量的空间机器人在轨捕获方法

Country Status (1)

Country Link
CN (1) CN115619828A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117193020A (zh) * 2023-10-12 2023-12-08 武汉科技大学 一种基于全自动液体样本处理工作站的目标识别和路径规划方法
CN117850244A (zh) * 2024-03-04 2024-04-09 海克斯康制造智能技术(青岛)有限公司 一种视觉测量控制系统及方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117193020A (zh) * 2023-10-12 2023-12-08 武汉科技大学 一种基于全自动液体样本处理工作站的目标识别和路径规划方法
CN117193020B (zh) * 2023-10-12 2024-05-07 武汉科技大学 一种基于全自动液体样本处理工作站的目标识别和路径规划方法
CN117850244A (zh) * 2024-03-04 2024-04-09 海克斯康制造智能技术(青岛)有限公司 一种视觉测量控制系统及方法
CN117850244B (zh) * 2024-03-04 2024-05-07 海克斯康制造智能技术(青岛)有限公司 一种视觉测量控制系统及方法

Similar Documents

Publication Publication Date Title
CN115619828A (zh) 一种基于模拟双目视觉测量的空间机器人在轨捕获方法
Papanikolopoulos et al. Six degree-of-freedom hand/eye visual tracking with uncertain parameters
Esrafilian et al. Autonomous flight and obstacle avoidance of a quadrotor by monocular SLAM
CN110116407A (zh) 柔性机器人位姿测量方法及装置
CN105910604A (zh) 一种基于多传感器的自主避障导航系统
CN107883965A (zh) 基于光学信息交互多模型强跟踪容积卡尔曼滤波导航方法
He et al. On the design and use of a micro air vehicle to track and avoid adversaries
CN110865650B (zh) 基于主动视觉的无人机位姿自适应估计方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
Vidal-Calleja et al. Action selection for single-camera SLAM
JP2003271975A (ja) 平面抽出方法、その装置、そのプログラム、その記録媒体及び平面抽出装置搭載型ロボット装置
Smith et al. Eye-in-hand robotic tasks in uncalibrated environments
CN111522362A (zh) 一种无线速度反馈的四旋翼无人机分布式编队控制方法
Nakath et al. Active Asteroid-SLAM: Active Graph SLAM with Landing Site Discovery in a Deep Space Proximity Operations Scenario
CN116460843A (zh) 一种基于元启发式算法的多机器人协作抓取方法及系统
Kawai et al. Passivity-based visual motion observer with panoramic camera for pose control
Ventura et al. Pose tracking of a noncooperative spacecraft during docking maneuvers using a time-of-flight sensor
CN110186465A (zh) 一种基于单目视觉的空间非合作目标相对状态估计方法
Cristofaro et al. Distributed information filters for MAV cooperative localization
Gardner et al. Pose and motion estimation of free-flying objects: Aerodynamics, constrained filtering, and graph-based feature tracking
CN114111772B (zh) 一种基于数据手套的水下机器人软体作业手位置跟踪方法
Zereik et al. Space robotics supporting exploration missions: vision, force control and coordination strategy for crew assistants
Pankert et al. Learning Contact-Based State Estimation for Assembly Tasks
CN112589795A (zh) 一种基于不确定性多帧融合的真空吸盘机械臂抓取方法
Hafez et al. Target model estimation using particle filters for visual servoing

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination