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

本发明专利技术一种基于视觉测量的空间机械臂轨迹规划方法，鉴于视觉敏感器测量数据更新周期与底层插补周期相差很大，首先基于视觉敏感器测量信息进行上层规划，然后基于上层规划的结果进行电机层面的下层规划，空间机械臂轨迹规划在这两个时间尺度交替滚动进行，不仅可以使空间机械臂在视觉敏感器测量信息引导之下运动至期望构型，而且能保证空间机械臂运动连续平稳，以便为视觉敏感器稳定成像创造有利条件，从而提高空间操作任务的成功率。

A path rolling planning method for space manipulator based on visual measurement

The invention relates to a visual measurement of space manipulator trajectory planning method based on visual sensor measurement data, given the update cycle and the interpolation cycle is different, the first visual sensor information based on the upper level, then the upper planning based on the results of the electrical machine is lower level planning, trajectory planning in the two time scale alternate rolling, not only can make the space manipulator to the desired configuration in visual sensor information, and ensure the continuous and steady motion, so as to create favorable conditions for the stability of the visual sensor imaging, so as to improve the success rate of task space operation.

【技术实现步骤摘要】
一种基于视觉测量的空间机械臂轨迹滚动规划方法
本专利技术属于空间机器人在轨维修维护领域，涉及一种基于视觉测量的空间机械臂运行轨迹的滚动规划方法。
技术介绍
在轨服务通常由配备有机械臂的追踪航天器(称之为空间机械臂系统)来完成。空间机械臂在视觉敏感器导引下实现对目标航天器的抓捕，并进行相应操作任务。机械臂轨迹的底层规划周期的时间尺度在亚毫秒级。然而，受制于目前星上图像处理硬件水平，视觉敏感器从采集目标图像到解算出目标相对于视觉敏感器坐标系的位置和姿态，通常需要几百毫秒左右的时间。机械臂底层轨迹规划周期与视觉敏感器位姿解算周期的不匹配会造成机械臂运动速度不连续，从而导致追踪航天器姿态抖动，影响视觉敏感器的成像质量，进而影响空间操作任务的成败。因此，在机械臂底层规划周期与视觉敏感器相对位姿解算周期不匹配的情况下，迫切需要一种能够实现空间机械臂运动连续平稳的轨迹规划方法。
技术实现思路
本专利技术解决的技术问题是：克服现有技术的不足，提供一种基于视觉测量的空间机械臂轨迹滚动规划方法，解决了空间机械臂底层轨迹规划周期与视觉敏感器位姿解算周期不匹配情况下的运动不平稳连续问题。本专利技术的技术方案是：一种基于视觉测量的空间机械臂轨迹滚动规划方法，步骤如下：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为机械臂末端执行器从初始位姿运动到目标位姿的时间规划，[]表示向下取整；上标本文档来自技高网...

【技术保护点】
一种基于视觉测量的空间机械臂轨迹滚动规划方法，其特征在于步骤如下：1)将视觉敏感器获取的目标位姿表示在空间机械臂基座航天器坐标系中；2)获取空间机械臂末端执行器的位姿偏差；3)对空间机械臂运动轨迹进行以视觉敏感器位姿解算周期为时间尺度的粗规划；计算获得视觉敏感器的相邻两个位姿解算周期之间的关节速度，以及在视觉敏感器下一采样周期的关节位置；4)对空间机械臂运动轨迹进行以关节电机插补周期为时间尺度的细规划；计算获得关节电机在视觉敏感器位姿解算周期之间的关节速度和关节位置。

【技术特征摘要】
1.一种基于视觉测量的空间机械臂轨迹滚动规划方法，其特征在于步骤如下：1)将视觉敏感器获取的目标位姿表示在空间机械臂基座航天器坐标系中；2)获取空间机械臂末端执行器的位姿偏差；3)对空间机械臂运动轨迹进行以视觉敏感器位姿解算周期为时间尺度的粗规划；计算获得视觉敏感器的相邻两个位姿解算周期之间的关节速度，以及在视觉敏感器下一采样周期的关节位置；4)对空间机械臂运动轨迹进行以关节电机插补周期为时间尺度的细规划；计算获得关节电机在视觉敏感器位姿解算周期之间的关节速度和关节位置。2.根据权利要求1所述的一种基于视觉测量的空间机械臂轨迹滚动规划方法，其特征在于：所述步骤(1)的具体方法为：根据姿态敏感器给出的基座航天器在惯性系中的姿态θ，将视觉敏感器测量得到的目标位姿表示在空间机械臂基座航天器坐标系中，记为pt和Rt，即pt表示目标在空间机械臂基座航天器坐标系中的位置，Rt表示目标坐标系相对于空间机械臂基座航天器坐标系的姿态矩阵；将空间机械臂末端执行器坐标系原点在空间机械臂基座航天器坐标系中的位置记为pm，将末端执行器坐标系相对于空间机械臂基座航天器坐标系的姿态矩阵记为Rm。3.根据权利要求2所述的一种基于视觉测量的空间机械臂轨迹滚动规划方法，其特征在于：所述获取空间机械臂末端执行器的位姿偏差的具体方法为：计算得到空间机械臂末端执行器的位置误差Δp＝pm-pt，姿态误差矩阵以ΔR对应的四元数的矢量部分表示空间机械臂末端执行器的姿态误差Δε，则空间机械臂末端执行器的位姿误差记为Δx＝[ΔpΔε]T；根据当前时刻tk空间机械臂基座航天器在惯性系中的姿态θ和机械臂各关节位置q，得到空间机械臂相对于空间机械臂基座航天器坐标系的雅克比矩阵J(θ,q)；角标k为自然数。4.根据权利要求3所述的一种基于视觉测量的空间机械臂轨迹滚动规划方法，其特征在于：所述步骤(3)的具体过程为：空间机械臂在视觉敏感器下一采样时刻tk+1＝tk+ΔT前的关节速度为

【专利技术属性】
技术研发人员：徐拴锋，魏春岭，何英姿，朱志斌，唐强，张军，周扬，
申请(专利权)人：北京控制工程研究所，
类型：发明
国别省市：北京,11