李新宇，邹纫秋，董 悫，王运来，孙羽佳
（1.中国空间技术研究院，北京 100094；
2.北京航天长征飞行器研究所，北京 100076；
3.北京理工大学 机电学院，北京 100081）
随着航天器研制技术的快速发展，航天器的结构和功能更加复杂，特殊装配操作需求日益增多。在航天器总装工作中，采用与人手臂自由度配置一致的双冗余臂机器人替代工人完成传统总装工作中的部分任务，可在保持原有生产模式、装配流程不变的同时提高装配质量和效率。然而，航天器装配场景具有非结构环境、舱内空间狭小等特点，受限空间下双臂机器人协同操作面临着非结构化环境难以建模等问题，无法保证双臂机器人在自主完成协同操作任务时的安全性[1]。因此，如何利用机器人的冗余自由度，针对装配环境限制，寻求机器人的最优轨迹规划是亟需解决的关键问题。同时，由于双冗余臂机器人的构型空间维度更高，协同操作任务约束复杂，具有较强的非线性，也给双臂机器人最优轨迹规划带来挑战[2-3]。
王美玲[4]将双臂协调操作分为双臂独立、双臂部分约束和双臂全约束3 种情况，分别给出了双臂之间、双臂与被操作物体之间的相对位姿约束关系和动力学约束关系。段晋军等[5]将任务空间正交解耦为外环的位置自由子空间和内环的力约束子空间，有效解决了双臂协调搬运过程中同时存在外部干扰和内力约束的问题。蒙磊等[6-7]利用改进后的粒子群算法进行冗余机械臂位置控制部分逆运动学计算，并基于灵活性能指标对机械臂进行参数优化，实现了大型板材安装。顾丹宁等[8]提出面向双臂机器人的改进RRT 方法，在关节空间中对机械臂进行路径规划，加入引力势场，提高规划效率。张子润[9]针对空间机器人在轨捕获和搬运组装等任务，采用高阶多项式插值和灵敏系数实现分别对左臂和右臂避碰规划。段嘉琪[10]针对在轨装配过程中的旋拧问题，提出了一种空间双臂机器人协调旋拧柔顺控制方法。杨志强[11]针对双臂机器人协调操作过程中给定路径的时间最优轨迹规划问题，采用基于杂交的粒子群算法对机械臂进行轨迹规划。张溯[12]提出一种以时间、能量、冲击为目标的多目标机械臂轨迹优化算法，实现了双臂动态协调轴孔装配。
目前对受限空间下的双冗余自由度机器人高效最优轨迹规划的研究相对较少。本文以双七自由度臂机器人的装配操作规划为目标，提出一种基于非线性约束优化方法的双臂机器人自主避碰与灵巧操作的最优轨迹规划方法，基于三维点云场景重建法建立受限作业环境的安全包络模型，构建双臂机器人协同操作的轨迹规划优化模型，以实现受限空间下双臂机器人协同操作优化模型的高效求解。
非结构化复杂环境下双冗余臂机器人与环境易发生碰撞等问题，严重威胁到机器人自身安全。本节将通过对作业场景的建模和表征，构建作业环境的安全包络，实现机器人主动避碰。研究所采用的机器人配备双七自由度机械臂，如图1 所示，为了实现对受限作业环境的感知，机器人须实时获取机械臂当前状态（双臂的2×7 个关节的角度和角速度）和末端相机的安装位姿，并根据坐标映射关系将环境模型转换至机器人世界坐标系，即
图1 双臂机器人系统中坐标系的变换关系Fig.1 Transformation relation of coordinate system in dualarm robot system
1.1 双冗余臂机器人环境感知
1.2 基于三维点云数据的安全包络模型
首先利用安装在双臂机器人右臂末端的RGBD相机，按照预设轨迹在安全移动范围内实现对未知作业场景的完整扫描，获得多帧受限作业场景的局部原始三维点云信息，根据点云数据构造局部几何特征；
然后利用局部几何特征找到旋转和平移参数，实现多帧点云数据配准；
最终通过多帧点云信息对作业环境进行三维重建，获得末端相机坐标系下完整作业场景的三维点云。
为了表征复杂环境中的安全工作区域的包络，采用基于扫掠球体法的环境障碍表征模型，对三维重建场景进行抽象表征。考虑到机械臂的连杆构型以及作业场景中三维点云的聚类结果，将双臂机器人的机械臂各连杆以及三维点云聚类出来的障碍特征抽象为线段扫掠体。线段扫掠球体是线段和球体的闵可夫斯基和，几何体A和几何体B的闵可夫斯基和可以定义为
其几何上的直观体现为，闵可夫斯基和是几何体A沿几何体B的边际连续运动1 周扫过的区域与几何体B本身的并集。如果A为球体，则生成的扫掠球体(A+B)的边界至几何体B的距离为球体A的半径r。具体地，考虑到机械臂的连杆构型，将机器人各连杆抽象为线段扫掠球体，即为线段和球体的闵可夫斯基和；
基于受限空间中安全区域边界的结合重构参数，构建平面点云的三角形扫掠球体，为三角形和球体的闵可夫斯基和，即受限环境下机器人协作空间的安全包络，如图2 所示。
1.3 安全包络间距离计算
基于扫掠球体法建立约束环境中的机器人协同操作的安全包络，采用计算图形学算法获得安全包络间的距离，即线段与线段间的空间距离。双臂机器人协同操作的安全性条件为包络模型间的距离大于0。包络模型间的距离可以通过平面几何体间的距离与各自球体半径的差值求得。
线段上任一点的几何参数表征公式为
式中：P0是线段的起点；
d0为线段起点至终点的单位矢量，即||d0||=1；
s∈[0, 1]，为线段位置参数，如图3 所示。
图3 线段与线段间空间距离计算示意Fig.3 Schematic diagram for calculating the spatial distance between line segments
线段与线段间的任一矢量可表示为d=L0(t)‒L1(t) = (P0–P1)+sd0‒td1，其中s∈[0, 1]、t∈[0, 1]，分别为端点在两条线段上的位置参数，因此矢量d的长度的平方为
考虑到参数s、t的取值范围，线段间的最近距离为一个含有边界约束的优化问题：
式(5)将线段间最近距离转化为一个含有边界约束的最优化问题，可以计算出不同条件下该问题最优解的解析形式[13]。
通过计算获得线段与线段间的最近距离d，若第k个扫掠体所采用的球体半径为rk，则扫掠球体i和扫掠体j之间的安全距离定义为Di j=d−ri−rj，则受限环境中双冗余臂机器人协同操作的安全约束数学模型为
针对非结构化约束环境中的双臂机器人灵巧协同操作任务，在第1 章对受限作业环境建模和线段扫掠球体包络间安全距离计算的基础上，提出一种基于最优化方法的双臂机器人安全轨迹快速规划方法，主要步骤包括：建立双臂机器人的安全约束和任务约束；
构建和求解轨迹规划的最优化问题。
2.1 安全约束
双臂机器人安全避碰地完成操作任务的条件为：1）双臂机器人自身各线段扫掠球体间的距离不小于0；
2）双臂机器人与受限作业空间的所有线段扫掠球体间的距离不小于0；
3）关节角度和角速度在安全限制范围内。
双臂机器人自身各线段扫掠球体间的距离与机器人的双臂状态有关：
其中x=[q;q˙]。
假设受限空间中的障碍是固定不变的，那么安全避碰条件也仅与机器人双臂状态有关：
机械臂各关节角度和角速度应当满足相应的物理限制：
即xmin≤x≤xmax。
综上，受限作业空间下双臂机器人协同操作的安全约束可以统一写为
2.2 任务约束
双臂机器人协同操作任务主要包括笛卡儿空间下的双臂操作任务和关节空间下的双臂操作任务。笛卡儿空间下的双臂协同操作任务约束主要是针对末端执行器的约束，包括完整末端路径约束、部分末端位姿约束以及双臂协同操作任务约束。完整末端路径约束完全确定了末端位姿，但对于七自由度冗余机械臂，仍有冗余自由度实现关节空间内的状态转移。为了计算机械臂末端位姿Tee(t)至末端约束路径的距离，使用距离当前末端位置pee(t)最近的路径约束点的位姿作为目标约束位姿，即机械臂的末端路径约束的数学模型为：
部分末端位姿约束与完整末端路径约束的数学模型相似，仅是对部分轴的方向进行约束，约束条件会相对更少，比如要求末端的第i个轴的方向在运动过程中保持不变，即
以双臂搬运任务为例的双臂协同操作的任务约束则需要考虑左右两机械臂末端间的相对位姿，在夹持物体后该相对位姿应当保持不变，因此有约束条件
关节空间任务约束主要为约束最终关节状态qterminal和terminal至目标值。针对机械臂关节运动至指定状态的任务要求，比如移动至目标关节角度qtarget，需要满足关节轨迹终端约束条件：
以上所有的任务约束条件都是等式约束的形式，因此，可以通过调整等式两侧的表达式，写成更为一般的任务约束模型
2.3 受限空间下双臂机器人最优轨迹规划数学模型
考虑到安全约束和双臂协同操作等约束条件，综合评价双臂机器人输入能量、轨迹长度和末端累积误差等因素，构建非结构化环境中双臂机器人协同操作的多约束非线性轨迹优化模型，如图4 所示。
图4 受限空间下机械臂最优轨迹规划数学模型Fig.4 Mathematical model for optimal trajectory planning of robotic arms in constrained space
2.4 基于约束差分动态规划的双臂机器人轨迹规划
2.3 节构建的受限空间下双臂机器人最优轨迹规划数学模型具有高维、多约束和非线性等特点，因此优化问题的求解会比较困难且耗时较长，影响双臂机器人开展操作任务的效率。为了提高轨迹规划效率，且保证轨迹的安全性以及最优性，本研究提出采用基于增广拉格朗日法的差分动态规划方法[14]：将多约束问题转化为无约束问题，并利用差分动态规划法实现高维规划问题的高效求解，获得满足安全避碰和任务约束的双臂操作最优轨迹。基于增广拉格朗日法的约束差分动态规划方法框架如图5 所示。
图5 基于增广拉格朗日法的约束差分动态规划法框架Fig.5 Framework of constrained differential dynamic programming method based on augmented Lagrange method
为验证本文提出的受限空间下双冗余臂机器人最优轨迹规划方法的可行性，选取典型的双臂机器人完成航天器装配的任务场景进行仿真和实验验证。轨迹规划方法的安全性和机器人的实际装配能力将在后续的装配任务实验中进行验证，因此本文仿真中进行了必要的简化，仅显示了机器人本体和双冗余度机械臂，便于观察机械臂的运动轨迹，隐藏复杂的末端执行器。
3.1 卫星配件装配仿真验证
仿真中采用编程语言Python 实现了相关规划算法的设计，采用MeshCat 作为机器人仿真的前端显示软件。卫星配件装配仿真中的轨迹规划长度（采样点数）均为N=500，采样间隔为dt=0.02 s，即轨迹总时长为10 s。双臂规划优化模型中的系数对角矩阵为
约束差分动态规划法中的初始惩罚因子μ=100，惩罚因子更新倍数为10。在卫星配件装配仿真中，不等式约束包括机械臂各臂的自避碰、双臂各连杆间的安全避碰以及双臂与目标卫星的安全避碰，等式约束主要为双臂末端间的闭链约束和双臂末端沿直线装配路径运动的路径约束。
具体地，双臂卫星配件装配的任务形式如图6所示：双臂共同抓取同一结构件，并通过结构件形成闭链；
双臂同时按照装配件与卫星上的销孔中心线移动，实施卫星配件的装配任务。在双臂轨迹规划中，同时具有双臂间形成的闭链约束和沿销孔中心线移动形成的路径约束，任务约束比较复杂。采用本文提出的最优轨迹规划方法所规划轨迹上的约束误差如图7 所示，双臂最大距离误差范数都小于7×10-7m，满足实际装配所需的精度，验证了本文最优轨迹规划方法的可行性。
图6 双臂机器人卫星配件装配任务仿真Fig.6 Simulation of satellite accessory assembly task for dual-arm robot
图7 双臂机器人末端执行器路径约束误差Fig.7 Path constraint error of end-effector of dual-arm robot
3.2 卫星配件装配实验验证
在3.1 节仿真验证结果的基础上，考虑实际的卫星和卫星配件的几何形状和抓取装配方式，设计基于本文双臂机器人最优轨迹规划方法的卫星配件装配实验验证。所采用的卫星模型和装配件如图8 所示：在卫星模型的正面有3 个装配销；
卫星装配件包括抓握点和装配面板，面板上有与卫星模型上装配销对应的装配孔。
图8 卫星模型与卫星装配件Fig.8 Satellite model and satellite assembly
先利用本文提出的双臂机器人最优轨迹规划方法生成满足双臂末端装配路径约束和双臂闭链约束的最优装配轨迹；
然后将该轨迹应用于双臂机器人实施卫星配件装配。装配过程如图9 所示，可以看到双臂机器人顺利完成了卫星装配件的抓取、移动和销孔对准装配，验证了本文最优轨迹规划方法的可行性。
图9 双臂机器人卫星配件装配过程Fig.9 Assembly process of dual-arm robot satellite accessories
本文针对受限空间下双臂机器人协同操作轨迹规划面临的非结构化环境复杂、作业空间小、易发生碰撞等问题，提出基于最优化方法的双臂机器人自主避碰与灵巧操作的轨迹规划方法：通过三维点云重建方法建立复杂约束环境的安全包络；
通过将安全作业和灵巧操作任务抽象为数学约束形式，将双臂协同规划问题转化为约束最优化问题，并利用差分动态规划方法求解双臂机器人在受限环境下自主避碰的最优轨迹。最后通过双七自由度机械臂机器人进行卫星配件装配任务的仿真和实验，验证了本文最优轨迹规划方法的可行性。