CN117001675A - 一种双臂协作操控非合作目标避障轨迹规划方法 - Google Patents

Abstract

本发明公开了一种双臂协作操控非合作目标避障轨迹规划方法，包括：首先建立空间操控航天器双机械臂外包络尺寸、DH参数，给定常规尺寸的非合作目标进行强化学习训练，获取避障路径，之后在实际任务场景中采用航天器视觉获取非合作目标外形轮廓，与训练的样本进行比对外形轮廓、双臂相对非合作目标的位置、姿态，如果实际场景中的非合作目标与样本库差距小，则对双机械臂采用样本库路径逼近非合作目标；如果差距较大，则对双机械臂采用胶囊体方法简化机械臂与非合作目标模型，并计算各自之间距离，以不碰撞作为约束规划航天器上双臂抓捕目标的路径，路径根据非合作目标与航天器的相对位置、姿态变化实时更新，解决非合作目标的避障操作问题。

Description

一种双臂协作操控非合作目标避障轨迹规划方法

技术领域

本发明属于空间机器人技术领域，特别涉及一种双臂协作操控非合作目标避障轨迹规划方法。

背景技术

国际上空间操控航天器一般配备机械臂，有些配置了双机械臂，这些航天器一般具备机械臂各关节及双臂之间的避障功能，但对于变化的场景，如太空中的非合作目标，航天器难以实现各关节不碰撞目标的情况下操作。

李宪华在“服务机器人双臂协作技术研究及实现”中提出了双臂拟人动作规划及双臂协作Petri网建模方法，属于已知环境下的规划方法，不适用于太空未知环境。

温秀兰在“基于双目视觉的双臂协作教学机器人研究与设计”设计了一种基于双目视觉的双臂协作教学机器人，该系统能够通过上位机拖动控制双臂运动完成示教编程，基于视觉引导图像处理及单臂抓取，不具备避障功能。

Hermann在文献“Unified GPU voxel collision detection for mobilemanipulation planning”中提出了基于视觉的自碰撞检测方法，该方法通过视觉检测机械臂自身关节是否碰撞，从而规划出无碰撞轨迹。该方法不具备非合作目标的避碰功能。

因此，现有技术并未解决空间非合作目标的避障操控问题。

发明内容

发明目的：为了克服以上不足，本发明的目的是提供一种双臂协作操控非合作目标避障轨迹规划方法，该方法便于实现在太空中替代航天员对未知的非合作目标进行避障操作，在保障航天员安全的同时提高工作效率，并具备操作的可靠性。

技术方案：为了实现上述目的，本发明提供一种双臂协作操控非合作目标避障轨迹规划方法，包括：

S1）：初始化，即设定空间操控航天器双臂结构参数；

S2）：根据步骤S1）设定的双臂结构参数，进行正运动学、雅克比矩阵、逆运动学求解；

S3）：给定常规尺寸的非合作目标，对双臂进行强化学习训练，获取避障路径，即采用SAC强化学习方法进行双臂的避障训练；

S4）：在实际任务场景中通过航天器视觉获取非合作目标外形轮廓、双臂相对非合作目标的位置、姿态；即相机对非合作目标进行观测，对非合作目标相邻帧特征点提取与匹配，再进行基于特征点对的帧间运动估计，计算特征点的投影误差判断位姿解算结果的准确性；

S5）：航天器视觉获取的非合作目标数据与训练的样本比对外形轮廓、双臂相对非合作目标的位置、姿态，如果实际场景中的非合作目标与样本库差距小，则对双机械臂采用样本库路径逼近非合作目标，如果差距较大，则转入步骤S6），即将测量得到的非合作目标数据与训练的样本进行比对，涉及到外形轮廓、双臂相对非合作目标的位置、姿态，如果实际场景中的非合作目标与样本库数据差距小于阈值n%，则对双机械臂采用样本库路径逼近非合作目标，如果数据差距大于阈值n%，则转入步骤S6）；

S6）：采用胶囊体方法简化机械臂与非合作目标模型；

S7）：计算机械臂各杆件之间的距离，以不碰撞作为约束规划航天器上双臂抓捕目标的路径，路径根据非合作目标的与航天器的相对位置、姿态变化实时更新；

S8）：完成空间操控航天器双臂对非合作目标的避障操作，即通过步骤S1）至S7）结合强化学习与胶囊体技术，解决双臂操作非合作目标的实时避障问题。

本发明所述的双臂协作操控非合作目标避障轨迹规划方法，所述步骤S1）设定空间操控航天机器人双臂结构参数，即为双臂的D-H参数，具体过程如下：

首先按如下规则建立D-H坐标系：

S101）：建立基座坐标系，左臂基座坐标系与右臂基座坐标系均可采用如下方式建立：

以基座上感兴趣的位置为原点、关节1（离本体最近的关节）的运动轴正方向为轴，建立右手正交坐标系/>，其中，/>轴和/>轴与/>轴垂直，方向任选；在建立左臂基座坐标系时感兴趣的位置为左臂基座与本体的交点；在建立右臂基座坐标系时感兴趣的位置为右臂基座与本体的交点；

S102）：对每个连杆i(i＝1、…，n-1)，完成S103）至S106）步；左臂的连杆1即为左臂基座；右臂的连杆1即为左臂基座；

S103）：每个连杆i建立一个坐标系；建立连杆i的坐标系的轴，即/>轴为关节轴（关节只能绕一个轴转动）：以关节i+1的运动(转动)轴正向为/>轴；（从近端到远端的关节序号依次加1）

S104）：建立连杆i坐标系的原点O _i ：若和/>轴相交，则以两轴交点为原点；若/>和/>轴异面或平行，则以两轴的公垂线与/>轴的交点为原点；

S105）：建立连杆i坐标系的轴，即/>轴：按/>建立/>轴，即使/>轴与/>轴及/>轴同时垂直；若/>轴与/>轴平行，则以它们的公垂线为/>轴；

S106）：建立连杆i坐标系的轴，即/>轴：根据已建立的/>轴和/>轴，按右手定则建立/>轴，即令/>；

定义：杆件扭角：绕/>轴转动，从/>轴旋转到/>轴的转角；

杆件长度：沿/>轴，从/>轴移动到/>轴的距离；

关节距离：沿/>轴，从/>轴移动到/>轴的距离；

关节转角：绕/>轴转动，从/>轴旋转到/>轴的角度。

本发明中所述的双臂协作操控非合作目标避障轨迹规划方法，所述步骤S2中正运动学、雅克比矩阵、逆运动学求解的具体过程如下： （1）正运动学求解如下

根据舱外机器人左、右机械臂DH参数，可得到左、右臂各关节的变换矩阵，依次对各关节的变换矩阵相乘，获得舱外机器人左臂或右臂的正运动学公式，详细过程如下；

用连杆变换矩阵来描述第i个关节坐标系在第i-1个关节坐标系中的位姿末端在基座中的位姿，/>表示连杆i坐标系相对于连杆i-1坐标系的变换，由连杆i坐标系依次经过以下四个子变换得到：

1）绕x _i-1轴转角；

2）沿x _i-1轴移动；

3）绕z _i 轴转角；

4）沿z _i 轴移动d _i ；

上述变换均相对于动坐标系描述，按照“从左向右”的原则，得到连杆变换的通式

式中，c表示cos，s表示sin；

将各个连杆变换相乘，可得机械臂变换矩阵/>

其中，n表示每个臂上的关节总数，由上式可知是n个关节变量的函数，表示末端坐标系相对于基坐标系的描述，至此可以得到机械臂的正运动学；

根据左臂DH参数，可得到左臂的正运动学变换矩阵为，根据右臂DH参数，可得到左臂的正运动学变换矩阵为/>；

（2）雅克比矩阵求解如下： 雅克比矩阵表示末端位姿速度与关节角速度的关系，可用于求解逆运动学，根据多个连杆变换/>的通式乘积求得，求解公式如下：

其中，，为4×4矩阵，/>表示/>中第1,2,3行与第3列交叉项，/>表示/>中第1,2,3行与第4列交叉项，/>表示/>中第1,2,3行与第4列交叉项；

当i=1时，为单位阵；

；

（3）逆运动学求解如下：

根据正运动学变换矩阵求解机械臂关节角，即为逆运动学；逆运动学求解可以利用雅克比矩阵将末端速度映射到关节空间获得关节速度，再积分便可获得关节位移；该方法可用于各种机械臂构型，具有良好的适应性，速度级运动学如下：

（1）

，表示第j+1个时刻的末端位姿变化量，即末端速度，其中，j=0，1,2，…，nt；J(j)表示第j个时刻的机械臂雅克比矩阵，Θ(j)表示第j个时刻的n个关节角/>，右角标T表示转置，/>，表示第j+1时刻的n个关节角/>变化量，即关节角速度；

关节角速度公式由上式求出，为

将左臂基座相对目标适配器的位姿序列X _lb (t)或右臂基座相对本体适配器的位姿序列X _rb (t)作为X(j+1) 的值；

关节角由上式求出为

（2）

根据上述逆运动学求解方法，可求得Θ(j+1)，即左臂的逆运动学解为Θ _l (t)，同理可求得右臂的逆运动学解为Θ _r (t)。

本发明所述步骤S3）中采用SAC强化学习方法进行双臂的避障训练，该方法是面向最大化熵开发的一种无模型算法，使用随机策略，让其在满足已知知识或者限定条件下，对未知的最好推断是随机不确定性的，各随机变量概率相等；

在SAC强化学习方法中，目标函数包含回报和策略熵，要求策略不仅能最大限度地提高最终回报，而且还要求最大化熵，SAC强化学习方法通过最大熵鼓励策略探索，为具有相近的Q网络的动作分配近乎均等的概率，不会给动作范围内任何一个动作分配非常高的概率，避免反复选择同一个动作而陷入次优；同时通过最大化奖赏，放弃明显没有前途的策略。

本发明所述的双臂协作操控非合作目标避障轨迹规划方法，所述步骤S4）中在实际任务场景中采用航天器视觉获取非合作目标外形轮廓、双臂相对非合作目标的位置、姿态，即采用航天器视觉对非合作目标进行观测，对非合作目标相邻帧特征点提取与匹配，再进行基于特征点对的帧间运动估计，计算特征点的投影误差判断位姿解算结果的准确性，其具体过程如下：

S401）：首先使用滤波模板计算像素点的增强灰度值；

S402）：基于图像灰度差异使用拉普拉斯算子对航天器视觉采集的卫星RGB图像进行边缘轮廓提取，并将提取的边缘轮廓叠加在原始图像中，实现边缘轮廓锐化突出非合作目标表面特征；

S403）：拉普拉斯算子通过比较中心像素的灰度和邻域像素点的平均灰度值大小，以提高对比度为原则增加或者降低中心像素点的灰度值，其中二维像素点的拉普拉斯算子表示为：

上述计算过程可以抽象为滤波模板，除此之外典型模板还有/>滤波模板、/>滤波模板、/>滤波模板，其中/>滤波模板、/>滤波模板通过四邻域滤波模板旋转45度与原模板相加得到；

使用滤波模板表示的拉普拉斯图像增强表达式为：

S404）：执行相邻帧的特征点提取与匹配以及匹配点对的筛选，匹配结果，进行基于特征点对的帧间运动估计，将相邻帧转换矩阵连乘得到非合作目标的位姿变换矩阵；

S405）：进行基于特征点对的帧间运动估计，将相邻帧转换矩阵连乘得到非合作目标的位姿变换矩阵；

即采用基于RANSAC的归一化八点法进行特征点对的帧间运动估计，获得相邻帧的转换矩阵。首先随机选取8个随机的特征点对，用归一化的坐标计算初始的归一化位姿变换矩阵转换矩阵，然后通过奇异值分解求解出满足秩为2的位姿变换矩阵。将所有相邻帧位姿变换矩阵连乘，便得到非合作目标的位姿变换矩阵；

S406）：根据位姿变换矩阵将初末时刻对应的两帧点云进行空间转换，如果非合作目标表面的点云重合即可验证位姿变换矩阵的正确性；即将初帧点云所有点的坐标左乘位姿变换矩阵，将得到由初帧点云获取的但与末帧点云同一位置和姿态的点云，两者重合，说明位姿变换矩阵的正确性。

本发明中所述步骤S404）中特征点提取是指通过尺度不变特征变换算法（scaleinvariant feature transform，SIFT）方法获取目标相邻帧的特征点，特征点包括关键点和描述子，首先利用高斯差分金字塔构造尺度空间，定位找到关键点，为关键点赋予指向特性，得到待匹配图像的SIFT描述子。

特征点匹配：得到待匹配图像的SIFT描述子后，计算相邻帧图像SIFT描述子之间的欧式距离，若最近距离与次近距离的比值小于设定的阈值，则认为两特征点之间可匹配；

匹配点对的筛选：类似在信号处理中用主成分分析法（principal componentsanalysisi，PCA）进行噪声去除的过程，将PCA引入匹配点对的筛选中，实现匹配点对的提纯，把错误的匹配点对去除。

本发明中所述的双臂协作操控非合作目标避障轨迹规划方法，所述步骤S6）中采用胶囊体方法简化机械臂与非合作目标模型的过程如下：

假设一个简单空间几何体将机械臂关节、连杆、非合作目标充分包容，由此将机械臂臂杆、关节以及非合作目标处简化为几何包络体；在双臂协作操控过程中，实时进行几何包络体之间的碰撞检测来推断双臂构件之间、双臂与非合作目标之间是否存在碰撞；

考虑到机械臂各个杆件可以近似为圆柱体，因此使用圆柱加半球的包络胶囊对机械臂进行简化建模；由于机械臂各部分排列紧凑，为避免某一连杆或关节被前后两个部件简化后的几何体完全覆盖的情况，将机械臂合理划分成几个子部分进行简化；

根据实际的机械臂构型，使用由中间为圆柱、两端为半球结构的包络胶囊，对单个机械臂进行结构简化；对非合作目标进行包络胶囊简化，采用一个长方体作为包络胶囊，包含非合作目标；

在建立单个机械臂的包络胶囊之后，进行碰撞对的设置；

首先定义右臂的包络胶囊序号为1,2,3...,n-1,n，其中，n为右臂的自由度数目，左臂的包络胶囊序号为a,b,c,...,N-1,N，其中，N为左臂的自由度数目；

根据机械臂构型设置可能发生碰撞的碰撞,双臂之间碰撞的碰撞对分别为：

2-b,2-c,2-(N-1),2-N,3-b,3-c,3-(N-1),3-N,4-b,4-c,4-(N-1),..,(n-1)-b,(n-1)-c,(n-1)-(N-1),(n-1)-N,n-b,n-c,n-(N-1),n-N

双臂与非合作目标T之间碰撞的碰撞对分别为：

1-T,2-T,3-T,...,(n-1)-T,n-T,a-T,b-T,c-T,..,(N-1)-T,N-T。

本发明中所述步骤S7）计算机械臂各杆件之间距离，以不碰撞作为约束规划航天器上双臂抓捕目标的路径，路径根据非合作目标与航天器的相对位置、姿态变化实时更新，由于初始位姿下机械臂与非合作目标之间不发生碰撞，因此运动过程中臂杆与非合作目标不发生碰撞的条件为：从初始时刻起各包络胶囊与非合作目标胶囊体最短距离值始终大于臂杆简化模型半径；具体过程如下： 操控机械臂一般为多自由度，超过六自由度则具有运动的冗余性；

利用机械臂冗余特性，进行双臂之间以及机械臂与非合作目标之间的实时避障轨迹规划，对于机械臂，其末端正运动学根据梯度投影法表示为如下形式：

其中是由各关节角速度构成的列矢量；/>为广义雅可比伪逆矩阵；/>为末端操作器的广义速度；/>为实标量放大系数；/>为单位矩阵，/>为雅可比矩阵，/>为优化指标，为优化指标/>的梯度，为使指标/>以最大速率增大或减小的关节速度矢量方向，表示如下：

避障规划利用项进行关节空间的运动来完成。

本发明所述避障规划采用的避障指标为最短距离指标，该指标通过遍历计算双臂之间各个臂杆的最短距离以及机械臂与非合作目标之间的最短距离，找到当前时刻机械臂构型下双臂臂杆中距离最近的点以及机械臂与非合作目标之间最近的点，并分别计算最短距离d_min；

用相应的最短距离与设定的安全距离阈值的比值来准确度量运动过程中两机械臂之间以及机械臂与非合作目标之间距离的危险程度；

当最短距离小于安全阈值d₀时，根据当前的最短距离计算机械臂的避障速度；

具体计算如下：

设沿最短距离对应臂杆上的点为C点，最短距离对应的单位向量为，则该点产生的臂杆躲避速度/>可用下式表示：

上式中为初始规划速度，通过机械臂杆件/>上C点对应的雅可比转置矩阵/>将臂杆处一点的躲避速度/>转化为各关节角速度/>，即为避障任务的优化指标梯度/>：

如果运动过程中同时出现多个碰撞对之间的最短距离小于安全阈值，则根据各臂杆对多个空间位置的躲避速度，通过雅可比转置矩阵将其转化为多个关节角速度进行累加得到总关节角速度，即为避障任务的优化指标梯度/>：

。

上述技术方案可以看出，本发明具有如下有益效果：

1、本发明所述的一种双臂协作操控非合作目标避障轨迹规划方法，通过强化学习与胶囊体方法相结合，即人工智能网络训练与显式方法相结合，即给出了预先训练结果，又能根据实际场景进行实时避障运算，是一种更加高效的方法，很好的解决了双机械臂抓捕非合作的自主避障问题。

2、本发明中所述的种双臂协作操控非合作目标避障轨迹规划方法通过胶囊体方法进行碰撞模型的简化，可实现目标与机械臂之间碰撞的自主检测，简化了其检测过程，让整个避障轨迹规划更为简单。

3、本发明通过航天器视觉获取非合作目标外形轮廓、双臂相对非合作目标的位置、姿态，视觉获取非合作目标外形轮廓有助于实现航天器在未知场景的无碰撞规划操作。

4、本发明中SAC算法整合了：演员-评论家、最大化熵模型框架，极大程度的解决了当前主流的无模型强化学习强化学习方法，在面向实际应用时都存在采样效率低和对超参数极其敏感的缺陷，相较于传统强化学习方法，也有着全新的贝尔曼方程表达式，同时在连续控制任务中有着非常出色的表现。

5、本发明中SAC通过最大熵鼓励策略探索，为具有相近的Q网络的动作分配近乎均等的概率，不会给动作范围内任何一个动作分配非常高的概率，避免反复选择同一个动作而陷入次优，在最大化奖赏的同时，鼓励探索，最大熵目标可以让动作更均匀的分布；还可以学到更多近优策略，提高了算法的鲁棒性，此外有效提高训练速度，且最大熵使探索更加均匀。

附图说明

图1是本发明提供的空间操控航天器抓捕非合作目标场景图；

图2是本发明提供的空间操控航天器双臂操作非合作目标避障轨迹规划策略流程图；

图3是本发明提供的机械臂DH参数示意图；

图4 是本发明提供的SAC强化学习方法双臂避障策略图；

图5 是本发明提供的图像轮廓增强效果对比图。

图6 是本发明提供的非合作目标特征点匹配与筛选结果图；

图7 是本发明提供的非合作目标点云配准结果图；

图8 是本发明提供的机械臂胶囊体图；

图9 是本发明提供的非合作目标胶囊体图；

图10 是本发明提供的双臂臂杆及非合作目标碰撞对图；

图11 是本发明提供的最短距离指标示意图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明。

实施例

如图所示的一种双臂协作操控非合作目标避障轨迹规划方法，包括：

S1）：初始化，即设定空间操控航天器双臂结构参数；

S2）：根据步骤S1）设定的双臂结构参数，进行正运动学、雅克比矩阵、逆运动学求解；

S3）：给定常规尺寸的非合作目标，对双臂进行强化学习训练，获取避障路径，即采用“轻型演员-评论家”（Soft Actor-Critic ，SAC）强化学习方法进行双臂的避障训练；

S4）：在实际任务场景中通过航天器视觉获取非合作目标外形轮廓、双臂相对非合作目标的位置、姿态，即采用航天器视觉对非合作目标进行观测，对非合作目标相邻帧特征点提取与匹配，再进行基于特征点对的帧间运动估计，计算特征点的投影误差判断位姿解算结果的准确性；

S5）：航天器视觉获取的非合作目标数据与训练的样本比对外形轮廓、双臂相对非合作目标的位置、姿态，如果实际场景中的非合作目标与样本库差距小，则对双机械臂采用样本库路径逼近非合作目标，如果差距较大，则转入步骤S6），即将测量得到的非合作目标数据与训练的样本进行比对，涉及到外形轮廓、双臂相对非合作目标的位置、姿态，如果实际场景中的非合作目标与样本库数据差距小于阈值n%，则对双机械臂采用样本库路径逼近非合作目标，如果数据差距大于阈值n%，则转入步骤S6）；

S6）：采用胶囊体方法简化机械臂与非合作目标模型；

S7）：计算机械臂各杆件之间距离，以不碰撞作为约束规划航天器上双臂抓捕目标的路径，路径根据非合作目标与航天器的相对位置、姿态变化实时更新；

S8）：完成空间操控航天器双臂对非合作目标的避障操作，即通过步骤S1）至S7）结合强化学习与胶囊体技术，解决双臂操作非合作目标的实时避障问题。

需要说明的是，本实施例中航天器视觉采用TOF相机，可以想到的是其也可以根据实际的需要选择其他合适的视觉检测机构，例如结构光相机或者双目相机。

本实施例中所述的双臂协作操控非合作目标避障轨迹规划方法，所述步骤S1）设定空间操控航天机器人双臂结构参数，即为双臂的D-H参数，具体过程如下：

首先按如下规则建立D-H坐标系如图3所示：

S101）：建立基座坐标系，左臂基座坐标系与右臂基座坐标系均可采用如下方式建立：

以基座上感兴趣的位置为原点、关节1（离本体最近的关节）的运动轴正方向为轴，建立右手正交坐标系/>，其中，/>轴和/>轴与/>轴垂直，方向任选；在建立左臂基座坐标系时感兴趣的位置为左臂基座与本体的交点；在建立右臂基座坐标系时感兴趣的位置为右臂基座与本体的交点；

S102）：对每个连杆i(i＝1、…，n-1)，完成S103）至S106）步；左臂的连杆1即为左臂基座；右臂的连杆1即为左臂基座；

S103）：每个连杆i建立一个坐标系；建立连杆i的坐标系的轴，即/>轴为关节轴（关节只能绕一个轴转动）：以关节i+1的运动(转动)轴正向为/>轴；（从近端到远端的关节序号依次加1）

S104）：建立连杆i坐标系的原点O _i ：若轴和/>轴相交，则以两轴交点为原点；若轴和/>轴异面或平行，则以两轴的公垂线与/>轴的交点为原点；

S105）：建立连杆i坐标系的轴，即/>轴：按/>建立/>轴，即使/>轴与/>轴及/>轴同时垂直；若/>轴与/>轴平行，则以它们的公垂线为/>轴；

S106）：建立连杆i坐标系的轴，即/>轴：根据已建立的/>轴和/>轴，按右手定则建立/>轴，即令/>；

定义：杆件扭角：绕/>轴转动，从/>轴旋转到/>轴的转角；

杆件长度：沿/>轴，从/>轴移动到/>轴的距离；

关节距离：沿/>轴，从/>轴移动到/>轴的距离；

关节转角：绕/>轴转动，从/>轴旋转到/>轴的角度。

本实施例中所述步骤S2中正运动学、雅克比矩阵、逆运动学求解的具体过程如下：（1）正运动学求解如下

根据舱外机器人左、右机械臂DH参数，可得到左、右臂各关节的变换矩阵，依次对各关节的变换矩阵相乘，获得舱外机器人左臂或右臂的正运动学公式，详细过程如下；

用连杆变换矩阵来描述第i个关节坐标系在第i-1个关节坐标系中的位姿末端在基座中的位姿，/>表示连杆i坐标系相对于连杆i-1坐标系的变换，由连杆i坐标系依次经过以下四个子变换得到：

1）绕轴转/>角；

2）沿轴移动/>；

3）绕轴转/>角；

4）沿轴移动/>；

上述变换均相对于动坐标系描述，按照“从左向右”的原则，得到连杆变换的通式

式中，c表示cos，s表示sin；

将各个连杆变换相乘，可得机械臂变换矩阵/>

其中，n表示每个臂上的关节总数，由上式可知是n个关节变量的函数，表示末端坐标系相对于基坐标系的描述，至此可以得到机械臂的正运动学；

根据左臂DH参数，可得到左臂的正运动学变换矩阵为，根据右臂DH参数，可得到右臂的正运动学变换矩阵为/>；

（2）雅克比矩阵求解如下： 雅克比矩阵表示末端位姿速度与关节角速度的关系，可用于求解逆运动学，根据多个连杆变换/>的通式乘积求得，求解公式如下：

其中，，为4×4矩阵，/>表示/>中第1,2,3行与第3列交叉项，/>表示/>中第1,2,3行与第4列交叉项，/>表示/>中第1,2,3行与第4列交叉项；

当i=1时，为单位阵；

；

（3）逆运动学求解如下：

根据正运动学变换矩阵求解机械臂关节角，即为逆运动学；逆运动学求解可以利用雅克比矩阵将末端速度映射到关节空间获得关节速度，再积分便可获得关节位移；该方法可用于各种机械臂构型，具有良好的适应性，速度级运动学如下：

（1）

，表示第j+1个时刻的末端位姿变化量，即末端速度，其中，j=0，1,2，…，nt；/>

表示第j个时刻的机械臂雅克比矩阵，Θ(j)表示第j个时刻的n个关节角，右角标T表示转置；/>表示j时刻机械臂末端位置姿态（x，y，z，alpha，beta，gama），其中alpha，beta，gama分别为连杆n坐标系x，y，z轴分别相对基座坐标坐标系x，y，z轴的转角；

，表示第j+1时刻的n个关节角变化量，即关节角速度；

关节角速度公式由上式求出，为

将左臂基座相对目标适配器的位姿序列X _lb (t)或右臂基座相对本体适配器的位姿序列X _rb (t)作为X(j+1) 的值；

关节角由上式求出为

（2）

根据上述逆运动学求解方法，可求得Θ(j+1)，即左臂的逆运动学解为Θ _l (t)，同理可求得右臂的逆运动学解为Θ _r (t)。

本实施例中所述步骤S3）中对双臂进行强化学习训练，获取避障路径，当前现有的主流的无模型强化学习强化学习方法，在面向实际应用时都存在着以下几点缺陷：

（1）采样效率低：由于每一次策略更新都需要在当前策略下重新采样足够多的样本数，完全抛弃之前的采样数据，需要非常高的样本数以及样本复杂性才能保证收敛。

（2）对超参数极其敏感：虽然使用经验回放解决了样本利用效率问题，但是策略与奖励值相互耦合，使其性能不稳定，容易受超参的影响。

本实施例中所述步骤S3）中采用SAC（Soft Actor-Critic ，轻型演员-评论家）强化学习方法进行双臂的避障训练的具体过程如下：

即采用SAC（Soft Actor-Critic ，轻型演员-评论家）强化学习方法进行双臂的避障训练，该方法是面向最大化熵开发的一种无模型算法，使用随机策略，让其在满足已知知识或者限定条件下，对未知的最好推断是随机不确定性的，各随机变量概率相等；该方法是面向最大化熵开发的一种无模型算法，使用随机策略，相比确定性策略具有一定的优势。

强化学习方法整合了：演员-评论家、最大化熵模型框架，极大程度的解决了以上强化学习算法的缺陷，相较于传统强化学习方法，也有着全新的贝尔曼方程表达式，同时在连续控制任务中有着非常出色的表现，应用于空间操控航天器双臂操作非合作目标避障的原理如图4所示。

从本质上来说，最大熵模型的意义就是：在满足已知知识或者限定条件下，对未知的最好推断是随机不确定性的（各随机变量等概率）。在强化学习算法中，希望策略能够尽可能的去探索环境，获得最优策略，但是如果策略输出为低熵的概率分布，则可能会贪婪采样某些值而陷入困境。为了在足够多的回报同时对未知状态空间进行合理的探索，选择最大熵强化学习模型：在满足限定条件下（获得足够多的回报），对未知状态空间等概率随机探索。

因此在SAC强化学习方法中，目标函数包含回报和策略熵，要求策略不仅能最大限度地提高最终回报，而且还要求最大化熵，SAC强化学习方法通过最大熵鼓励策略探索，为具有相近的Q网络的动作分配近乎均等的概率，不会给动作范围内任何一个动作分配非常高的概率，避免反复选择同一个动作而陷入次优；同时通过最大化奖赏，放弃明显没有前途的策略。

总的来说最大熵的好处就是：

（1）在最大化奖赏的同时，鼓励探索（最大熵目标可以让动作更均匀的分布，因为当策略输出的动作为等概率时，熵最大）

（2）可以学到更多近优策略，提高了算法的鲁棒性

（3）训练速度加快（最大熵使探索更加均匀）

对于最大熵的强化学习可以得到贝尔曼方程，此时可获取价值函数，

Q网络参数可以通过优化最小化与估计值的差来得到。

本实施例中所述步骤S4中在实际任务场景中采用航天器视觉获取非合作目标外形轮廓、双臂相对非合作目标的位置、姿态，即采用航天器视觉对非合作目标进行观测，对非合作目标相邻帧特征点提取与匹配，再进行基于特征点对的帧间运动估计，计算特征点的投影误差判断位姿解算结果的准确性；具体过程如下：

S401）：首先使用滤波模板计算像素点的增强灰度值；

S402）：基于图像灰度差异使用拉普拉斯算子对航天器视觉采集的卫星RGB图像进行边缘轮廓提取，并将提取的边缘轮廓叠加在原始图像中，实现边缘轮廓锐化突出非合作目标表面特征；

S403）：拉普拉斯算子通过比较中心像素的灰度和邻域像素点的平均灰度值大小，以提高对比度为原则增加或者降低中心像素点的灰度值，其中二维像素点的拉普拉斯算子表示为：

上述计算过程可以抽象滤波模板，除此之外典型模板还有/>滤波模板、/>滤波模板、/>滤波模板，其中/>滤波模板、/>滤波模板通过四邻域滤波模板旋转45度与原模板相加得到；

使用滤波模板表示的拉普拉斯图像增强表达式为：

使用滤波模板计算像素点的增强灰度值，经过拉普拉斯处理后的边缘轮廓如图5中的（a）图所示，将提取的边缘轮廓区域图像与原图（图5中的（b）图）相加，得到的轮廓边界增强效果如图5中的（c)图所示，可见经拉普拉斯算子处理后卫星模型的轮廓特征更加明显，提高了后续特征提取与匹配的初始输入图像的质量；

执行相邻帧的特征点提取与匹配以及匹配点对的筛选，匹配结果如图6所示；进行基于特征点对的帧间运动估计，将相邻帧转换矩阵连乘得到非合作目标的位姿变换矩阵；需要说明的是： 位置姿态变换矩阵*初始帧目标位置姿态=最后帧目标位置姿态

位姿匹配的结果经位姿变换，根据相机位姿转换矩阵将初末时刻对应的两帧点云进行空间转换与拼接如图7所示，可见非合作目标表面的点云基本重合；

S404）：执行相邻帧的特征点提取与匹配以及匹配点对的筛选；

特征点提取：通过尺度不变特征变换算法（scale invariant featuretransform，SIFT）方法获取目标相邻帧的特征点，特征点包括关键点和描述子，首先利用高斯差分金字塔构造尺度空间，定位找到关键点，为关键点赋予指向特性，得到待匹配图像的SIFT描述子。

特征点匹配：得到待匹配图像的SIFT描述子后，计算相邻帧图像描述子之间的欧式距离，若最近距离与次近距离的比值小于设定的阈值，则认为两特征点之间可匹配；

匹配点对的筛选：类似在信号处理中用主成分分析法（principal componentsanalysisi，PCA）进行噪声去除的过程，将PCA引入匹配点对的筛选中，实现匹配点对的提纯，把错误的匹配点对去除；

S405）：进行基于特征点对的帧间运动估计，将相邻帧转换矩阵连乘得到非合作目标的位姿变换矩阵；

采用基于RANSAC的归一化八点法进行特征点对的帧间运动估计，获得相邻帧的转换矩阵；首先随机选取8个随机的特征点对，用归一化的坐标计算初始的归一化位姿变换矩阵转换矩阵，然后通过奇异值分解求解出满足秩为2的位姿变换矩阵。将所有相邻帧位姿变换矩阵连乘，便得到非合作目标的位姿变换矩阵；

S406）：根据位姿变换矩阵将初末时刻对应的两帧点云进行空间转换，如果非合作目标表面的点云重合即可验证位姿变换矩阵的正确性；即将初帧点云所有点的坐标左乘位姿变换矩阵，将得到由初帧点云获取的但与末帧点云同一位置和姿态的点云，两者重合，说明位姿变换矩阵的正确性。

本实施例中所述的双臂协作操控非合作目标避障轨迹规划方法，所述步骤S6）中采用胶囊体方法简化机械臂与非合作目标模型的过程如下：

假设一个简单空间几何体将机械臂关节、连杆、非合作目标充分包容，由此将机械臂臂杆、关节以及非合作目标处简化为几何包络体；在双臂协作操控过程中，实时进行几何包络体之间的碰撞检测来推断双臂构件之间、双臂与非合作目标之间是否存在碰撞；

考虑到机械臂各个杆件可以近似为圆柱体，因此使用圆柱加半球的包络胶囊对机械臂进行简化建模；由于机械臂各部分排列紧凑，为避免某一连杆或关节被前后两个部件简化后的几何体完全覆盖的情况，将机械臂合理划分成几个子部分进行简化；

如图8所示根据实际的机械臂构型，使用由中间为圆柱、两端为半球结构的包络胶囊，对单个机械臂进行的结构简化；

如图9所示对非合作目标进行包络胶囊简化，采用一个长方体作为包络胶囊，包含非合作目标。

在建立单个机械臂的包络胶囊之后，进行碰撞对的设置；

如图10所示，首先定义右臂的包络胶囊序号为1,2,3...,n-1,n，

其中，n为右臂的自由度数目，左臂的包络胶囊序号为a,b,c,...,N-1,N，其中，N为左臂的自由度数目；

根据机械臂构型设置可能发生碰撞的碰撞,双臂之间碰撞的碰撞对分别为：

2-b,2-c,2-(N-1),2-N,3-b,3-c,3-(N-1),3-N,4-b,4-c,4-(N-1),..,(n-1)-b,(n-1)-c,(n-1)-(N-1),(n-1)-N,n-b,n-c,n-(N-1),n-N

双臂与非合作目标T之间碰撞的碰撞对分别为：

1-T,2-T,3-T,...,(n-1)-T,n-T,a-T,b-T,c-T,..,(N-1)-T,N-T。

本实施例中所述的双臂协作操控非合作目标避障轨迹规划方法，所述步骤S7）中计算机械臂各杆件之间距离，以不碰撞作为约束规划航天器上双臂抓捕目标的路径，路径根据非合作目标与航天器的相对位置、姿态变化实时更新，由于初始位姿下机械臂与非合作目标之间不发生碰撞，因此运动过程中臂杆与非合作目标不发生碰撞的条件为：从初始时刻起各包络胶囊与非合作目标胶囊体最短距离值始终大于臂杆简化模型半径；具体过程如下：

操控机械臂一般为多自由度，超过六自由度则具有运动的冗余性；

利用机械臂冗余特性，进行双臂之间以及机械臂与非合作目标之间的实时避障轨迹规划，对于机械臂，其末端正运动学根据梯度投影法表示为如下形式：

其中是由各关节角速度构成的列矢量；/>为广义雅可比伪逆矩阵；/>为末端操作器的广义速度；/>为实标量放大系数；/>为单位矩阵，/>为雅可比矩阵，/>为优化指标，为优化指标/>的梯度，为使指标/>以最大速率增大或减小的关节速度矢量方向，表示如下：

避障规划利用项进行关节空间的运动来完成。

本实施例中所述的双臂协作操控非合作目标避障轨迹规划方法，所述避障规划采用的避障指标为最短距离指标，该指标通过遍历计算双臂之间各个臂杆的最短距离以及机械臂与非合作目标之间的最短距离，找到当前时刻机械臂构型下双臂臂杆中距离最近的点以及机械臂与非合作目标之间最近的点，并分别计算最短距离d_min如图11所示；

用相应的最短距离与设定的安全距离阈值的比值来准确度量运动过程中两机械臂之间以及机械臂与非合作目标之间距离的危险程度；

当最短距离小于安全阈值d₀时，根据当前的最短距离计算机械臂的避障速度；

具体计算如下：

设沿最短距离对应臂杆上的点为C点，最短距离对应的单位向量为，则该点产生的臂杆躲避速度/>可用下式表示：/>

上式中为初始规划速度，通过机械臂杆件/>上C点对应的雅可比转置矩阵/>将臂杆处一点的躲避速度/>转化为各关节角速度/>，即为避障任务的优化指标梯度/>：

如果运动过程中同时出现多个碰撞对之间的最短距离小于安全阈值，则根据各臂杆对多个空间位置的躲避速度，通过雅可比转置矩阵将其转化为多个关节角速度进行累加得到总关节角速度，即为避障任务的优化指标梯度/>：

。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进，这些改进也应视为本发明的保护范围。

Claims (9)

1.一种双臂协作操控非合作目标避障轨迹规划方法，其特征在于：包括：

S1）：初始化，即设定空间操控航天器双臂结构参数；

S2）：根据步骤S1）设定的双臂结构参数，进行正运动学、雅克比矩阵、逆运动学求解；

S3）：给定常规尺寸的非合作目标，对双臂进行强化学习训练，获取避障路径，即采用SAC强化学习方法进行双臂的避障训练；

S4）：在实际任务场景中通过航天器视觉获取非合作目标外形轮廓、双臂相对非合作目标的位置、姿态，即采用航天器视觉对非合作目标进行观测，对非合作目标相邻帧特征点提取与匹配，再进行基于特征点对的帧间运动估计，计算特征点的投影误差判断位姿解算结果的准确性；

S5）：航天器视觉获取的非合作目标数据与训练的样本比对外形轮廓、双臂相对非合作目标的位置、姿态，如果实际场景中的非合作目标与样本库差距小，则对双机械臂采用样本库路径逼近非合作目标；如果差距较大，则转入步骤S6）；即将测量得到的非合作目标数据与训练的样本进行比对，涉及到外形轮廓、双臂相对非合作目标的位置、姿态，如果实际场景中的非合作目标与样本库数据差距小于阈值n%，则对双机械臂采用样本库路径逼近非合作目标，如果数据差距大于阈值n%，则转入步骤S6）；

S6）：采用胶囊体方法简化机械臂与非合作目标模型；

S7）：计算机械臂各杆件之间距离，以不碰撞作为约束规划航天器上双臂抓捕目标的路径，路径根据非合作目标与航天器的相对位置、姿态变化实时更新；

S8）：完成空间操控航天器双臂对非合作目标的避障操作，即通过步骤S1）至S7）结合强化学习与胶囊体技术，解决双臂操作非合作目标的实时避障问题。

2.根据权利要求1所述的双臂协作操控非合作目标避障轨迹规划方法，其特征在于：所述步骤S1）设定空间操控航天机器人双臂结构参数，即为双臂的D-H参数，具体过程如下：

首先按如下规则建立D-H坐标系：

S101）：建立基座坐标系，左臂基座坐标系与右臂基座坐标系均可采用如下方式建立：以基座上感兴趣的位置为原点、关节1的运动轴正方向为/>轴，建立右手正交坐标系，其中，/>轴和/>轴与/>轴垂直，方向任选；在建立左臂基座坐标系时感兴趣的位置为左臂基座与本体的交点；在建立右臂基座坐标系时感兴趣的位置为右臂基座与本体的交点；

S102）：对每个连杆i，完成下方步骤S103）至步骤S106）；左臂的连杆1即为左臂基座；右臂的连杆1即为右臂基座；

S103）：每个连杆i建立一个坐标系；建立连杆i的坐标系的轴，即 />轴为关节轴；以关节i+1的运动轴正向为/>轴；

S104）：建立连杆i坐标系的原点：若/>轴和/>轴相交，则以两轴交点为原点；若/>轴和轴异面或平行，则以两轴的公垂线与/>轴的交点为原点；

S105）：建立连杆i坐标系的轴，即/>轴；按/>建立/>轴，即使/>轴与轴及/>轴同时垂直；若/>轴与/>轴平行，则以它们的公垂线为/>轴；

S106）：建立连杆i坐标系的轴，即/>轴；根据已建立的/>轴和/>轴，按右手定则建立/>轴，即令/>；

定义：杆件扭角：绕/>轴转动，从/>轴旋转到/>的转角；

杆件长度：沿/>轴，从/>轴移动到/>轴的距离；

关节距离：沿/>轴，从/>轴移动到/>轴的距离；

关节转角：绕/>轴转动，从/>轴旋转到/>轴的角度。

3.根据权利要求2所述的双臂协作操控非合作目标避障轨迹规划方法，其特征在于：所述步骤S2）中正运动学、雅克比矩阵、逆运动学求解的具体过程如下： （1）正运动学求解如下

根据舱外机器人左、右机械臂DH参数，可得到左、右臂各关节的变换矩阵，依次对各关节的变换矩阵相乘，获得舱外机器人左臂或右臂的正运动学公式，详细过程如下：

用连杆变换矩阵 来描述第i个关节坐标系在第i-1个关节坐标系中的位姿末端在基座中的位姿，/>表示连杆i坐标系相对于连杆i-1坐标系的变换，由连杆i坐标系依次经过以下四个子变换得到：

1）绕轴转/>角；

2）沿轴移动/>；

3）绕轴转/>角；

4）沿轴移动/>；

上述变换均相对于动坐标系描述，按照“从左向右”的原则，得到连杆变换矩阵的通式为

式中，c表示cos，s表示sin；

将各个连杆变换相乘，可得机械臂变换矩阵/>

其中，表示每个臂上的关节总数，由上式可知/>是/>个关节变量的函数，表示末端坐标系相对于基坐标系的描述，至此可以得到机械臂的正运动学；

根据左臂DH参数，可得到左臂的正运动学变换矩阵为，根据右臂DH参数，可得到右臂的正运动学变换矩阵为/>；

（2）雅克比矩阵求解如下： 雅克比矩阵表示末端位姿速度与关节角速度的关系，可用于求解逆运动学，根据多个连杆变换/>的通式乘积求得，求解公式如下：

其中，，为4×4矩阵，/>表示/>中第1,2,3行与第3列交叉项，表示/>中第1,2,3行与第4列交叉项，/>表示/>中第1,2,3行与第4列交叉项；

当i=1时，为单位阵，

；

（3）逆运动学求解如下：

逆运动学求解可以利用雅克比矩阵将末端速度映射到关节空间获得关节速度，再积分便可获得关节位移；该方法可用于各种机械臂构型，具有良好的适应性，速度级运动学如下：

（1）

，表示第j+1个时刻的末端位姿变化量，即末端速度，其中，j=0，1,2，…，nt；J(j)表示第j个时刻的机械臂雅克比矩阵；Θ(j)表示第j个时刻的n个关节角，右角标T表示转置；/>，表示第j+1时刻的n个关节角变化量，即关节角速度；

关节角速度公式由上式求出，为

将左臂基座相对目标适配器的位姿序列X _lb (t)或右臂基座相对本体适配器的位姿序列X _rb (t)作为X(j+1)的值；

关节角由上式求出为

（2）

根据上述逆运动学求解方法，可求得Θ(j+1)，即左臂的逆运动学解为Θ _l (t)，同理可求得右臂的逆运动学解为Θ _r (t)。

4.根据权利要求1所述的双臂协作操控非合作目标避障轨迹规划方法，其特征在于：所述步骤S3）中采用SAC强化学习方法进行双臂的避障训练，该方法是面向最大化熵开发的一种无模型算法，使用随机策略，让其在满足已知知识或者限定条件下，对未知的最好推断是随机不确定性的，且各随机变量概率相等；

在SAC强化学习方法中，目标函数包含回报和策略熵，要求策略不仅能最大限度地提高最终回报，而且还要求最大化熵，SAC强化学习方法通过最大熵鼓励策略探索，为具有相近的Q网络的动作分配近乎均等的概率，不会给动作范围内任何一个动作分配非常高的概率，避免反复选择同一个动作而陷入次优；同时通过最大化奖赏，放弃明显没有前途的策略。

5.根据权利要求1所述的双臂协作操控非合作目标避障轨迹规划方法，其特征在于：所述步骤S4）中采用航天器视觉对非合作目标进行观测，对非合作目标相邻帧特征点提取与匹配，再进行基于特征点对的帧间运动估计，计算特征点的投影误差判断位姿解算结果的准确性，其具体过程如下：

S401）：首先使用滤波模板计算像素点的增强灰度值；

S402）：基于图像灰度差异使用拉普拉斯算子对航天器视觉采集的卫星RGB图像进行边缘轮廓提取，并将提取的边缘轮廓叠加在原始图像中，实现边缘轮廓锐化突出非合作目标表面特征；

S403）：拉普拉斯算子通过比较中心像素的灰度和邻域像素点的平均灰度值大小，以提高对比度为原则增加或者降低中心像素点的灰度值，其中二维像素点的拉普拉斯算子表示为：

上述计算过程可以抽象为滤波模板，除此之外典型模板还有/>滤波模板、/>滤波模板、滤波模板，其中/>滤波模板、/>滤波模板通过四邻域滤波模板旋转45度与原模板相加得到；

使用滤波模板表示的拉普拉斯图像增强表达式为：

S404）：执行相邻帧的特征点提取与匹配以及匹配点对的筛选；

S405）：进行基于特征点对的帧间运动估计，将相邻帧转换矩阵连乘得到非合作目标的位姿变换矩阵；

S406）：根据位姿变换矩阵将初末时刻对应的两帧点云进行空间转换，如果非合作目标表面的点云重合即可验证位姿变换矩阵的正确性；即将初帧点云所有点的坐标左乘位姿变换矩阵，将得到由初帧点云获取的但与末帧点云同一位置和姿态的点云，两者重合，说明位姿变换矩阵的正确性。

6.根据权利要求5所述的双臂协作操控非合作目标避障轨迹规划方法，其特征在于：所述步骤S404）中执行相邻帧的特征点提取与匹配以及匹配点对的筛选的具体过程如下：

特征点提取：通过尺度不变特征变换算法方法获取目标相邻帧的特征点，特征点包括关键点和描述子，首先利用高斯差分金字塔构造尺度空间，定位找到关键点，为关键点赋予指向特性，得到SIFT描述子；

特征点匹配：得到待匹配图像的SIFT描述子后，计算相邻帧图像SIFT描述子之间的欧式距离，若最近距离与次近距离的比值小于设定的阈值，则认为两特征点之间可匹配；

匹配点对的筛选：类似在信号处理中PCA用主成分分析法进行噪声去除的过程，将PCA引入匹配点对的筛选中，实现匹配点对的提纯，把错误的匹配点对去除。

7.根据权利要求1所述的双臂协作操控非合作目标避障轨迹规划方法，其特征在于：所述步骤S6）中采用胶囊体方法简化机械臂与非合作目标模型的过程如下：

假设一个简单空间几何体将机械臂关节、连杆、非合作目标充分包容，由此将机械臂臂杆、关节以及非合作目标处简化为几何包络体；

在双臂协作操控过程中，实时进行几何包络体之间的碰撞检测来推断双臂构件之间、双臂与非合作目标之间是否存在碰撞；

考虑到机械臂各个杆件可以近似为圆柱体，因此使用圆柱加半球的包络胶囊对机械臂进行简化建模；由于机械臂各部分排列紧凑，为避免某一连杆或关节被前后两个部件简化后的几何体完全覆盖的情况，将机械臂合理划分成几个子部分进行简化；

根据实际的机械臂构型，使用由中间为圆柱、两端为半球结构的包络胶囊，对单个机械臂进行结构简化；

对非合作目标进行包络胶囊简化，采用一个长方体作为包络胶囊，包含非合作目标；

在建立单个机械臂的包络胶囊之后，进行碰撞对的设置，即首先定义右臂的包络胶囊序号为1,2,3...,n-1,n，其中，n为右臂的自由度数目，左臂的包络胶囊序号为a,b,c,...,N-1,N，其中，N为左臂的自由度数目；

根据机械臂构型设置可能发生碰撞的碰撞,双臂之间碰撞的碰撞对分别为：

2-b,2-c,2-(N-1),2-N,3-b,3-c,3-(N-1),3-N,4-b,4-c,4-(N-1),...,(n-1)-b,(n-1)-c,(n-1)-(N-1),(n-1)-N,n-b,n-c,n-(N-1),n-N

双臂与非合作目标T之间碰撞的碰撞对分别为：

1-T,2-T,3-T,...,(n-1)-T,n-T,a-T,b-T,c-T,...,(N-1)-T,N-T。

8.根据权利要求1所述的双臂协作操控非合作目标避障轨迹规划方法，其特征在于：所述步骤S7）中计算机械臂各杆件之间距离，以不碰撞作为约束规划航天器上双臂抓捕目标的路径，路径根据非合作目标与航天器的相对位置、姿态变化实时更新，由于初始位姿下机械臂与非合作目标之间不发生碰撞，因此运动过程中臂杆与非合作目标不发生碰撞的条件为：

从初始时刻起各包络胶囊与非合作目标胶囊体最短距离值始终大于臂杆简化模型半径；具体过程如下：

利用机械臂冗余特性，进行双臂之间以及机械臂与非合作目标之间的实时避障轨迹规划，对于机械臂，其末端正运动学根据梯度投影法表示为如下形式：

其中，是由各关节角速度构成的列矢量；/>为广义雅可比伪逆矩阵；/>为末端操作器的广义速度；/>为实标量放大系数；/>为单位矩阵，/>为雅可比矩阵，/>为优化指标，/>为优化指标/>的梯度，为使指标/>以最大速率增大或减小的关节速度矢量方向，表示如下：

避障规划利用项进行关节空间的运动来完成。

9.根据权利要求8所述的双臂协作操控非合作目标避障轨迹规划方法，其特征在于：所述避障规划采用的避障指标为最短距离指标，该指标通过遍历计算双臂之间各个臂杆的最短距离以及机械臂与非合作目标之间的最短距离，找到当前时刻机械臂构型下双臂臂杆中距离最近的点以及机械臂与非合作目标之间最近的点，并分别计算最短距离d_min；

用相应的最短距离与设定的安全距离阈值的比值来准确度量运动过程中两机械臂之间以及机械臂与非合作目标之间距离的危险程度；

当最短距离小于安全阈值d₀时，根据当前的最短距离计算机械臂的避障速度；

具体计算如下：

设沿最短距离对应臂杆上的点为C点，最短距离对应的单位向量为，则该点产生的臂杆躲避速度/>可用下式表示：

上式中为初始规划速度，通过机械臂杆件/>上C点对应的雅可比转置矩阵/>将臂杆处一点的躲避速度/>转化为各关节角速度/>，即为避障任务的优化指标梯度/>：

如果运动过程中同时出现多个碰撞对之间的最短距离小于安全阈值，则根据各臂杆对多个空间位置的躲避速度，通过雅可比转置矩阵将其转化为多个关节角速度进行累加得到总关节角速度，即为避障任务的优化指标梯度/>：

。