一种基于视觉测量的空间机械臂轨迹滚动规划方法与流程

本发明属于空间机器人在轨维修维护领域，涉及一种基于视觉测量的空间机械臂运行轨迹的滚动规划方法。

背景技术：

在轨服务通常由配备有机械臂的追踪航天器(称之为空间机械臂系统)来完成。空间机械臂在视觉敏感器导引下实现对目标航天器的抓捕，并进行相应操作任务。机械臂轨迹的底层规划周期的时间尺度在亚毫秒级。然而，受制于目前星上图像处理硬件水平，视觉敏感器从采集目标图像到解算出目标相对于视觉敏感器坐标系的位置和姿态，通常需要几百毫秒左右的时间。机械臂底层轨迹规划周期与视觉敏感器位姿解算周期的不匹配会造成机械臂运动速度不连续，从而导致追踪航天器姿态抖动，影响视觉敏感器的成像质量，进而影响空间操作任务的成败。因此，在机械臂底层规划周期与视觉敏感器相对位姿解算周期不匹配的情况下，迫切需要一种能够实现空间机械臂运动连续平稳的轨迹规划方法。

技术实现要素：

本发明解决的技术问题是：克服现有技术的不足，提供一种基于视觉测量的空间机械臂轨迹滚动规划方法，解决了空间机械臂底层轨迹规划周期与视觉敏感器位姿解算周期不匹配情况下的运动不平稳连续问题。

本发明的技术方案是：一种基于视觉测量的空间机械臂轨迹滚动规划方法，步骤如下：

1)将视觉敏感器获取的目标位姿表示在空间机械臂基座航天器坐标系中；

2)获取空间机械臂末端执行器的位姿偏差；

3)对空间机械臂运动轨迹进行以视觉敏感器位姿解算周期为时间尺度的粗规划；计算获得视觉敏感器的相邻两个位姿解算周期之间的关节速度，以及在视觉敏感器下一采样周期的关节位置；

4)对空间机械臂运动轨迹进行以关节电机插补周期为时间尺度的细规划；计算获得关节电机在视觉敏感器位姿解算周期之间的关节速度和关节位置。

所述步骤(1)的具体方法为：

根据姿态敏感器给出的基座航天器在惯性系中的姿态θ，将视觉敏感器测量得到的目标位姿表示在空间机械臂基座航天器坐标系中，记为pt和rt，即pt表示目标在空间机械臂基座航天器坐标系中的位置，rt表示目标坐标系相对于空间机械臂基座航天器坐标系的姿态矩阵；将空间机械臂末端执行器坐标系原点在空间机械臂基座航天器坐标系中的位置记为pm，将末端执行器坐标系相对于空间机械臂基座航天器坐标系的姿态矩阵记为rm。

所述获取空间机械臂末端执行器的位姿偏差的具体方法为：

计算得到空间机械臂末端执行器的位置误差δp＝pm-pt，姿态误差矩阵以δr对应的四元数的矢量部分表示空间机械臂末端执行器的姿态误差δε，则空间机械臂末端执行器的位姿误差记为δx＝[δpδε]^t；根据当前时刻tk空间机械臂基座航天器在惯性系中的姿态θ和机械臂各关节位置q，得到空间机械臂相对于空间机械臂基座航天器坐标系的雅克比矩阵；角标k为自然数。

所述步骤(3)的具体过程为：

空间机械臂在视觉敏感器下一采样时刻tk+1＝tk+δt前的关节速度为

其中，为目标的实际运动速度，包括线速度与角速度；δt为视觉敏感器的位姿数据更新周期；λ为正定对称矩阵；位姿误差反馈系数其中t表示时间，t为机械臂末端执行器从初始位姿运动到目标位姿的时间规划，[]表示向下取整；上标⁺表示经典违逆；

空间机械臂各关节在时刻tk+1的关节位置为

在时刻tk+1，空间机械臂关节位置由下式近似给出

其中，q(tk)为空间机械臂在时刻tk的关节位置；

采用以下方法计算视觉敏感器的相邻两个位姿解算周期之间的关节速度

其中，n＝[t/δt]为两个周期之间的路径点个数，[]表示向下取整，和分别为起始点和结束点对应的关节速度，为第k个路径点对应的关节速度，给出[tk-1,tk]时段内的轨迹斜率。

所述计算获得关节电机在视觉敏感器位姿解算周期之间的关节速度和关节位置的具体过程为：

根据空间机械臂在时刻tk和tk+1的关节位置q(tk)和q(tk+1)，在时刻tk和tk+1的关节速度和以及各关节从初始位置q(tk)运动到末了位置q(tk+1)的时间δt，进行底层插补，插补周期为δt。

所述底层插补选择梯形规划算法、s形规划算法、带抛物线过渡的梯形规划算法、多项式规划算法以及样条规划算法。

本发明与现有技术相比的优点在于：

(1)本发明公开的规划算法分为两个层次，上层规划为底层规划提供约束条件，两个层次的规划相互独立，可以分别采取不同的规划算法，灵活性强；

(2)本发明公开的规划算法引入了位姿误差反馈系数的概念，可以使机械臂运动更加平稳，从而为视觉敏感器成像创造了有利条件；

(3)本发明公开的规划算法可扩展应用于视觉敏感器位姿解算周期变化的情况，适应性强。

附图说明

图1为试验流程框图；

图2为试验全程的机械臂末端三轴位置误差曲线；

图3为试验最后30s的机械臂末端三轴位置误差曲线；

图4为试验全程的机械臂末端坐标系的误差四元数变化曲线；

图5为试验最后30s的机械臂末端坐标系的误差四元数变化曲线。

具体实施方式

基于搭建的空间非合作目标抓捕操作地面试验系统，验证本发明所公开的滚动规划算法，机械臂采用串联结构，末端测量设备为手眼双目相机。考虑到安全性，机械臂开始操作前，关闭其基座卫星平台的姿轨控系统，成为自由漂浮空间机械臂。当目标星可抓捕部位处于视觉敏感器视场中，且敏感器能够测量得到可抓捕部位的相对位姿时，空间机械臂进入视觉导引模式。由相机测量目标星上可抓捕部位在相机坐标系中的相对期望位姿，然后进行上层规划得到机械臂在手眼双目相机下一采样时刻的关节位置和关节速度，接着进行底层规划得到电机运动的位置速度，从而控制机械臂末端移动到目标可抓捕部位，完成对目标航天器的捕获。

本发明公开了一种基于视觉测量的空间机械臂轨迹滚动规划方法，步骤如下：

一种基于视觉测量的空间机械臂轨迹滚动规划方法，其特征在于步骤如下：

1)将视觉敏感器获取的目标位姿表示在空间机械臂基座航天器坐标系中。根据姿态敏感器给出的基座航天器在惯性系中的姿态θ，将视觉敏感器测量得到的目标位姿表示在空间机械臂基座航天器坐标系(即卫星本体系)中，记为pt和rt，即pt表示目标在空间机械臂基座航天器坐标系中的位置，rt表示目标坐标系相对于空间机械臂基座航天器坐标系的姿态矩阵；将空间机械臂末端执行器坐标系原点在空间机械臂基座航天器坐标系中的位置记为pm，将末端执行器坐标系相对于空间机械臂基座航天器坐标系的姿态矩阵记为rm；

2)获取空间机械臂末端执行器的位姿偏差。计算得到空间机械臂末端执行器的位置误差δp＝pm-pt，姿态误差矩阵以δr对应的四元数的矢量部分表示空间机械臂末端执行器的姿态误差δε，则空间机械臂末端执行器的位姿误差记为δx＝[δpδε]^t；根据当前时刻tk空间机械臂基座航天器在惯性系中的姿态θ和机械臂各关节位置q，得到空间机械臂相对于空间机械臂基座航天器坐标系的雅克比矩阵j(θ,q)；角标k为自然数；

3)对空间机械臂运动轨迹进行以视觉敏感器位姿解算周期为时间尺度的粗规划；计算获得视觉敏感器的相邻两个位姿解算周期之间的关节速度，以及在视觉敏感器下一采样周期的关节位置，具体为：

空间机械臂在视觉敏感器下一采样时刻tk+1＝tk+δt前的关节速度为

其中，为目标的实际运动速度，包括线速度与角速度；δt为视觉敏感器的位姿数据更新周期；λ为正定对称矩阵；位姿误差反馈系数其中t表示时间，t为机械臂末端执行器从初始位姿运动到目标位姿的时间规划，[]表示向下取整；上标⁺表示经典违逆；

空间机械臂各关节在时刻tk+1的关节位置为

在时刻tk+1，空间机械臂关节位置可由下式近似给出

其中，q(tk)为空间机械臂在时刻tk的关节位置；

采用以下方法计算视觉敏感器的相邻两个位姿解算周期之间的关节速度

其中，n＝[t/δt]为两个周期之间的路径点个数，[]表示向下取整，和分别为起始点和结束点对应的关节速度，为第k个路径点对应的关节速度，给出[tk-1,tk]时段内的轨迹斜率；

(4)对空间机械臂运动轨迹进行以关节电机插补周期为时间尺度的细规划；计算获得关节电机在视觉敏感器位姿解算周期之间的关节速度和关节位置，具体为：

根据空间机械臂在时刻tk和tk+1的关节位置q(tk)和q(tk+1)，在时刻tk和tk+1的关节速度和以及各关节从初始位置q(tk)运动到末了位置q(tk+1)的时间δt，进行底层插补，插补周期为δt(通常δtδt)；根据实际需要，底层插补算法可以选择梯形规划算法、s形规划算法、带抛物线过渡的梯形规划算法、多项式规划算法以及样条规划算法。

本发明方法实施例所涉及的试验系统包括一部固定基座六自由度机械臂，机械臂末端安装双目手眼相机，为机械臂的抓捕操作提供测量信息。

图1为试验流程框图。试验开始前，机械臂处于某预先定义的构型(即零位构型)，当手眼双目相机能够稳定获取目标星上待抓捕典型部位的位姿后，试验开始。首先，将手眼双目相机的测得的典型部位位姿表示在机械臂坐标系0中，进行上层规划，得到机械臂在相机下一采样时刻的关节位置和速度；然后，以上层规划结果为约束条件，进行底层规划，得到电机运动的位置速度。

图2和图3分别为试验全程和试验最后30s的机械臂末端三轴位置误差曲线。由图2可知，当时间大于67s后，三轴位置误差均在5mm以内。由图3可知，当时间大于133s后，三轴位置误差均在2mm以内，符合位置容差要求。由图2和图3可以看出，机械臂末端三轴位置误差收敛较快，且机械臂运动平稳。

图4和图5分别给出了试验全程和试验最后30s的机械臂末端坐标系误差四元数变化曲线。由图4可知，在58s后，误差四元数快速收敛。由图5可知，当时间大于133s后，误差四元数均在0.0021以内，符合姿态容差要求。由图4和图5可以看出，机械臂末端坐标系误差四元数收敛较快，且机械臂末端姿态运动平稳。物理仿真试验结果表明，本发明提出的基于手眼双目相机测量信息的空间机械臂轨迹滚动规划方法能够使机械臂运动较为平稳，并有效地提高空间操作任务的成功率。本发明的主要技术内容可应用于在空间机器人在轨维修维护任务，行星表面探测机器人机械臂抓取操作任务等。

本发明未详细说明部分属本领域技术人员公知常识。