２０１２年６月 中国空间科学技术 ４９ 第 ３期 Ｃｈｉｎｅｓｅ Ｓｐａｃｅ Ｓｃｉｅｎｃｅ ａｎｄ Ｔｅｃｈｎｏｌｏｇｙ 冗余空间机械臂粗捕获段无碰撞轨迹规划算法 王悦 贾英宏 徐世杰 （北京航空航天大学宇航学院，北京１００１９１） 摘要 现有的基于ｃ空间的无碰撞轨迹规划算法需要求解Ｃ空间以获得ｃ空间障碍边 界。对于多自由度的冗余机械臂，求解过程需要消耗很大的计算量和内存，不适用于计算 资源紧张的空间机械臂轨迹的快速规划。文章提出了一种不需要求解ｃ空间的试探性规划 算法，包含４个子算法：碰撞检测算法、无碰撞目标构型求解算法、无碰撞路径搜索算法 和路径平滑算法。已知期望的末端作用器位置和姿态，利用目标构型求解算法得到无碰撞 的目标构型，然后由路径搜索算法在ｃ空间障碍边界未知的情况下，利用碰撞检测算法， 采用一定的试探规律，在Ｃ空间中搜索出一条无碰撞路径，最终由路径平滑算法使该路径 平滑，易于实现。仿真算例表明，该算法是快速有效的，适用于冗余空间机械臂粗捕获段 的快速轨迹规划。 关键词 冗余机械臂 轨迹规划 避碰 Ｃ空间 ＤＯＩ：１０．３ｚ８ｏ／ｊ．ｉｓｓｎ．１０００—７５８Ｘ．２Ｏ１２．０３．００８ １ 引言 空间机械臂在航天任务中得到重要和广泛的应用。空间机械臂与航天器本体存在动力学耦合， 捕获目标时要考虑目标的相对运动。可行的简化是将捕获分为粗捕获段和细捕获段，粗捕获段不考 虑目标相对运动，简化为固定基座机械臂的轨迹规划。规避障碍是轨迹规划的一个关键问题，冗余 机械臂因具有额外自由度用以规避障碍而得到重视。 冗余机械臂的无碰撞轨迹规划算法主要有伪逆算法、人工势场法和基于ｃ空间的自由空间 法口］。伪逆算法利用雅克比矩阵ｔ，的伪逆Ｊ 得到关节角速度，并由零空问投影（Ｉ—ｔ，＋ｔ，）得到齐 次解来规避障碍Ｅ ］，求解中易出现奇异ｌ＿３］。人工势场法存在局部最小值问题，且对于复杂问题，计 算量过大 ］，较适用于移动机器人的轨迹规划ｌ＿４］。ｃ空间即构型空间，构型点的坐标为机械臂的各 关节角。由运动学关系将作用器目标位姿和障碍物由工作空问映射到ｃ空间，得到目标构型点和ｃ 空间障碍，该障碍是机械臂与障碍物碰撞的构型集合。无碰撞轨迹规划即要在ｃ空间中搜索出连 接初始和目标构型点且不经过障碍的路径。为了减小计算量，提出了多种方法：采用简化的机械臂 模型Ｅ ；基于ｃ空间变换特性的Ｃ空间障碍边界高效计算方法 等。但上述方法都需求解ｃ空间， 对于多自由度机械臂计算量很大，ｃ空间障碍只能通过离散化得剑，为提高精度需占用大量内 存 ］。这导致算法无法应用于计算资源紧张的空间机械臂。而新近提出的基于神经网络 和增强现 实技术 的规划方法较为复杂，难以应用于航天任务。 为了将基于ｃ空问的无碰撞轨迹规划算法应用于空间机械臂，以７ Ａ由度机械臂为例，针对 国家自然科学基金（１０８７２０２８）资助项目 收稿日期：２Ｏｌ１－０７—１２。收修改稿日期：２Ｏ１１ ０９—１３ § 主垦至 旦型堂垫 笙 旦 相对于本体静止的障碍物，提出一种试探性规划算法，该算法无需求解Ｃ空间，通过碰撞检测就 可以在Ｃ空间中找出无碰撞路径。该算法也可应用于月球车车载机械臂等航天任务。 ２算法的结构 Ｃ空间中的轨迹规划，就是在Ｃ空间中找到一条始于初始构型点Ｓ到达目标构型点Ｅ，且不 穿过ｃ空间障碍的运动轨迹。这个过程包括２个步骤（如图１所示）： １）找到一个满足作用器目标位姿，且不与障碍物相碰的目标构型点Ｅ。７自由度机械臂满足 目标位姿的构型在Ｃ空间中是一条曲线，如图１中的ＡＢ，可能会有某些构型是与障碍物相碰的， 这些构型点位于ｃ空间障碍的内部，如曲线ＡＢ的虚线部分，显然，目标构型点Ｅ必须位于ｃ空 间障碍之外。 ２）在求得目标构型点Ｅ之后，要找到一条连接Ｓ和Ｅ，且不与Ｃ空间障碍相交的曲线ＳＥ， 这是一个７维空问中的路径搜索过程。现有算法的缺陷在于需要求解Ｃ空间，即求解Ｃ空间障碍 Ｃ１、Ｃ２的边界。 虽然求解Ｃ空间比较困难，但在已知工作空间中障碍物位置的情况下，检测在某种构型下机 械臂是否与障碍物相碰是很简单的，这样就可以尝试不求解Ｃ空间，在Ｃ空间中障碍分布未知的 情况下，通过碰撞检测算法，采用某种特定的算法不断试探，从而找到一条无碰撞的路径。 图２是算法的基本结构图，包含了４个子算法：碰撞检测算法、无碰撞目标构型求解算法、无 碰撞路径搜索算法和路径平滑算法。无碰撞目标构型求解算法完成步骤１，无碰撞路径搜索算法和 路径平滑算法完成步骤２，碰撞检测算法被这３个子算法调用。算法的输人为期望的末端作用器位 姿和当前机械臂构型，前者由操作任务给出，后者可由各关节角测量得到，算法的输出为需要的无 碰撞路径。无碰撞路径搜索算法和路径平滑算法是该算法的核心，这里简要介绍碰撞检测算法和无 碰撞目标构型求解算法。 图１ 机械臂Ｃ空间轨迹规划不意 Ｆｉｇ．１ Ｃｏｌｌｉｓｉｏｎ－ｆｒｅｅ ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ ｉｎ Ｃ—ｓｐａｃｅ 图２ 轨迹规划算法的流程 Ｆｉｇ．２ Ｓｅｑｕｅｎｃｅ ｏｆ ｔｈｅ ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ ａｌｇｏｒｉｔｈｍ 在一定的构型下，机械臂上各点相对于基座的位置容易求得，而障碍物相对于基座的位置也是 已知的，碰撞检测算法可以如此实现：对于凸障碍物可以用一个凸多面体包络代替，而非凸障碍物 总可以用多个凸多面体包络代替，然后将机械臂离散化，选取足够多的检测点，进而检测每个点是 否在障碍物包络内。这就简化为检测点是否在凸多面体内，这是计算几何的基础问题。可以在凸多 面体的每个面上选取一个点，然后计算由待检测点指向该点的矢量与凸多面体该面的外法向量的内 积。如果对于凸多面体的所有面，这样的内积全为正，则待检测点在凸多面体内部。只要存在一个 待检测点在其中一个凸多面体内，则认为该待检测点位于障碍物内部，即该构型下机械臂是与障碍 物碰撞的。 至 旦 主垦奎 型兰垫 § 无碰撞目标构型求解算法需要求解机械臂的逆运动学，再调用碰撞检测算法对逆运动学的解进 行判断，如果解是碰撞的，则需要求逆运动学的另一组解。这种对逆运动学解的搜索可以通过以下 方法实现：固定某一个关节转角为 ，然后求解其余６个关节角的逆运动学，那么就得到０ 的一 个解，不断改变０ 就能对逆运动学的解进行搜索，直至找到一个无碰撞的解。 ３无碰撞路径搜索算法和路径平滑算法 以７自由度机械臂为例，详细介绍一种无需求解 ｃ空间的试探性路径搜索算法和与之配合的路径平滑算 法。首先注意到在Ｃ空间有这样一种特殊的路径：连接 ｓ点和Ｅ点的折线路径。如图３所示，折线路径由平行于 坐标轴的线段拼接而成，每个线段代表沿某个坐标轴的 １个子运动，这种子运动意味着机械臂的一个关节角在运 动，其余关节角保持不变。所以在这种折线路径上机械臂 总是只有一个关节角在运动。 显然，如果ｓ和Ｅ之间存在无碰撞的路径，那么总可 以用一些足够小的子运动按合适的顺序拼接成一个折线形 图３ Ｃ空间中的无碰撞折线路径 的无碰撞路径。搜索算法的目的是找到这些子运动及其排 Ｆｉｇ．３ Ｃｏｌｌｉｓｉｏｎ－ｆｒｅｅ ｐｅｒｐｅｎｄｉｃｕｌａｒ ｐａｔｈ ｉｎ Ｃ—ｓｐａｃｅ 列顺序。进一步假设Ｓ和Ｅ之问存在无碰撞的直接路径 （该路径上各个关节角的运动都是单向的），则情况更加简单。此时令正整数Ｎ为路径分割常数， 将各个关节角从ｓ点到Ｅ点的总转角均分成』＼，份，关节角转过每份转角定义为一个子运动，就形 成了７Ｎ个子运动。那么只要Ｎ足够大，将路径分割得足够细，就可以用这７Ｎ个子运动按合适的 顺序拼接成一个无碰撞折线路径ＳＥ。 记第ｉ个关节角的子运动为子运动ｉ（ 一１，２，…，７），搜索算法的任务是通过不断试探确定 这７Ｎ个子运动的顺序。即将总路径分成７Ｎ步，将７Ｎ个子运动以合适的顺序排列，形成一个７Ｎ 步的运动序列。 设定序号生成函数Ｆ（ ），该函数由当前子运动序号ｉ生成下一步要试探的子运动序号。Ｆ（ ） 可以根据需要选取，如递增、递减或随机生成。用ｍ表示步号，Ｔ表示试探总次数，Ｔ 表示可接 受最大试探次数，算法步骤如下： １）初始化，ｍ一１，Ｔ一１，选取ｉ，设置Ｔ…。 ２）若ｍ＝＝＝７Ｎ＋１，算法成功结束，输出结果；否则利用碰撞检测算法，判断机械臂第 步执 行子运动ｉ是否与障碍物碰撞。如果不碰撞，机械臂执行子运动ｉ（这是在计算机中的模拟），并做 记录，执行步骤３）；如果碰撞，执行步骤４）。 ３）ｍ—ｍ＋１，ｉ—Ｆ（ ），Ｔ—Ｔ＋１，执行步骤６）。 ４）判断第ｍ步是否已经试探了所有可能的子运动，若是，执行步骤５）；否则ｉ—Ｆ（ ），Ｔ＝＝＝ Ｔ＋１，执行步骤６）。 ５）跳回第ｍ一１步，令ｍ—ｍ一１。判断第ｍ步是否已经试探了所有可能的子运动，若是重新 执行步骤５）；否则令ｉ—Ｆ（ （ ））， （ ）指第 步原来采用的子运动序号，丁一Ｔ＋１，执行步骤６）。 ６）若Ｔ—Ｔ ，算法失败结束，否则执行步骤２）。 若该算法搜索失败，则可以采用不同的试探规律Ｆ（ ）或增大路径分割常数Ｎ重新搜索。试探规 律Ｆ（ ）可以有多种，若采用随机试探，每次试探的结果都会不同。试探规律的多样性，保证了搜索 算法的有效性。增大路径分割常数Ｎ则可以增大搜索的成功率，但是这是以增加计算量为代价的。 由前面的描述可知，该路径搜索算法的前提是将总路径分割成许多小段。并且从理论上来讲， 分割得越细，则无碰撞路径搜索的成功率就越高。然而，这种由许多小段组成的路径对于机械臂的 生旦窒回 兰堇 生 旦 后续控制是不利的。因为，即使每一个子运动的关节角运动规律采用高次多项式给出，由许多个子 运动组成的总路径的关节角的运动规律也是不够平滑的。并且在上面的折线路径上，机械臂总是只 有一个关节角在运动，导致操作时间会比较长。注意到，在实际操作中机械臂只有在某些段才会可 能与障碍物相碰。在远离障碍物的运动区域，不必要将运动分成很多段的子运动。所以，这里提出 种路径平滑算法，对上面搜索到的无碰撞折线路径进行平滑处理。 一算法的基本思路如下：尝试将上面搜索到的无碰撞折线路径中相邻的两个子运动合并成一个子 运动，然后检测合并后的子运动是否与障碍物相碰。如果不相碰，则将这两个子运动合并；若相碰 则取消合并。不断尝试，直至得到的运动序列中相邻的子运动无法进一步合并。 至此，已经得到了算法的两个核心子算法：无碰撞路径搜索算法和路径平滑算法，下面选取算 例对算法的有效性进行验证。 ４ 算例 首先以３维空问中的搜索为例，验证无碰撞路径搜索算法的有效性，然后以７自由度机械臂为 例验证无碰撞路径搜索算法和路径平滑算法的有效性。 ４．１ ３维空间无碰撞路径搜索 假定３维空间中的一个点，在不了解障碍物的分布只能 判断某一个运动是否与障碍物相碰的前提下，利用第３节 表１算例１的障碍物参数 Ｔａｂ．１ Ｐａｒａｍｅｔｅｒｓ ｏｆ ｔｈｅ ｏｂｓｔａｃｌｅｓ Ｚ Ｏ ８ ６ 提出的搜索算法，搜索一条由起始点Ｓ（０，０，０）到目标点 Ｅ（１０，１０，１０）的无碰撞路径。障碍物为５个棱边与坐标轴 障碍物 ３２范围 ｙ范围 ．ｚ范围 第１个长方体 Ｏ～６ ２～５ ２～４ １～７ ２～８ 平行的长方体，坐标范围见表１。取路径分割常数Ｎ一１０。 第２个长方体 ４～９ ４～５若序号生成函数取为Ｆ（ ）一 ＋１，再对３求余，即递 第３个长方体 ０～５．５ ６～７ 增试探，经６５次试探，算法得到一个无碰撞的运动序列： 第４个长方体 ６～１０ ２～９ １，２，３，１，２，１，２，１，１，１，１，３，１，３，１，３，１，２，２，２，２，２，２，２，３， ５～８ １～４ ３，３，３，３，３。若函数Ｆ（ ）在子运动序号中随机选择，经 第５个长方体 ４～８ ８～９ ３７次试探算法得到一个无碰撞的运动序列：１，３，３，３，２，１，３，３，１，１，３，１，３，３，１，１，１，１，１，３，２，２，３， ２，２，２，２，２，２，２。需要注意，函数Ｆ（ ）采用随机选取时，算法每次给出的结果是不同的。这两个结 果分别如图４和图５所示。算例证实了试探性无碰撞路径搜索算法的有效性。 １０ ８ ６ Ｚ ４ ２ ０ １０ Ｏ Ｏ 起始点 起始点 图４递增试探路径搜索过程 Ｆｉｇ．４ Ｐａｔｈ ｓｅａｒｃｈｉｎｇ ｂｙ ｓｅｑｕｅｎｔｉａｌ ｔｅｓｔ 图５ 随机试探路径搜索过程 Ｆｉｇ．５ Ｐａｔｈ ｓｅａｒｃｈｉｎｇ ｂｙ ｒａｎｄｏｍ ｔｅｓｔ ２０１２年６月 中国空间科学技术 ４．２ ７自由度机械臂无碰撞轨迹规划 以美国“轨道快车”（Ｏｒｂｉｔａｌ Ｅｘｐｒｅｓｓ）上的机械臂为原型，设计了一个７自由度机械臂。机 械臂的几何参数如表２所示，其中第１节臂安装点在基座坐标系中表示。各个关节角为０时的初始 构型以及各个转角的正方向见图６，此时基座坐标系和各节臂的体坐标系重合，见图中的坐标系 ０一ｘｙｚ。第７节臂即是末端作用器。下面以该机械臂为例验证本文算法的有效性。 表２各节臂的安装点位置参数 各节臂 安装点在前一节 臂体坐标系中的坐标／ｍ １节臂 ２节臂 ３节臂 ４节臂 （０．６５，０，０．６９２８） （０，０．１２，０．１） （一１，０．１２，Ｏ） （１．１５，０．１２，０） ５节臂 ６节臂 ７节臂 （０，０，一０．１５） （０．１５，０，０） （Ｏ，一０．１，Ｏ） 图６机械臂的初始构型和转轴正方向 Ｆｉｇ．６ Ｉｎｉｔｉａｌ ｃｏｎｆｉｇｕｒａｔｉｏｎ ａｎｄ ｄｉｒｅｃｔｉｏｎｓ ｏｆ ｊｏｉｎｔ ａｘｅｓ 设机械臂的初始构型为ｑ ，各关节角为（一１．５７０ ７，３．１４１ ４，１，０，０，０，０）ｒａｄ。以下参 数在基座坐标系中描述：期望末端作用器端点位置为（２．０，０．３，０．５）ｍ；期望末端作用器轴线 方向余弦为（Ｏ．４６９ ９，一０．１９８ ７，０．８６０ １）；障碍物为一个棱边与坐标轴平行的长方体，ｚ、Ｙ、ｚ 坐标范围分别为１．２～１．７ｍ、０～０．５ １ＴＩ、０～０．５ ｍ。注意，目标位姿对末端作用器沿轴线的旋转 没有约束，且第７节臂轴线与第７关节角转轴平行，所以第７关节角不会发生运动，实质上是一个 ６自由度机械臂满足５自由度的末端作用器位姿要求。 作为对比，首先采用伪逆算法 一Ｊ。。 进行规划，其中 为末端作用器位姿变化率列向量， 为关节角速度列向量， 中各个量取为半个周期的正弦曲线，结果显示得到的路径是与障碍物相碰 的，见图７。图中显示了机械臂在空间扫过的区域，说明在该情况下，避障问题是需要考虑的。 下面采用本文提出的算法，首先由无碰撞目标构型求解算法，求解出一个满足末端作用器位姿 要求且不与障碍物相碰的目标构型ｑＥ，各关节角为（一０．０５６７，４．６１２７，１．４５６９，～２．１６９１， ０．９５６１，一０．３０２４，０）ｒａｄ，求解过程这里不再详述。接着由无碰撞路径搜索算法搜索出一条不与 障碍物相碰的路径，路径分隔常数Ｎ一１０，第７关节角转角没有变化，实质上是６维空间的搜索问 题，共分成６Ｎ一６０个子运动，函数Ｆ（ ）采用递增试探。经过６４次试探，找到一个无碰撞的运动 序列：１，２，３，４，５，６，１，２，３，４，５，６，１，２，３，４，５，６，１，２，３，４，５，６，１，２，３， ４，５，６，１，２，３，４，５，６，１，２，３，４，５，６，２，３，４，５，６，２，３，４，５，６，２，３，４，５， ６，１，１，１。可以看到序列的前面部分与试探顺序相同，没有进行调整，在序列的后面算法对试探 顺序进行调整以避开障碍。机械臂的运动过程见图８，机械臂从底部绕过障碍物到达了目标构型， 验证了该算法的有效性。Ｆ（ ）尝试不同的试探方式可以提高搜索成功率，而路径分割常数Ｎ需要 在计算量和成功率之间权衡，这显示了该算法的灵活性。每一段子运动运动时间为４ Ｓ，关节角运 动规律采用多项式，使得每个子运动开始和结束时，关节角速度和角加速度为０。图９给出了各个 关节角的角速度曲线。从图９可以看出，关节角速度从总体来看是不够平滑的。从上面的搜索过程 看，在大部分运动过程中，机械臂不会与障碍物相碰。 ５４ 中国空间科学技术 ２０１２年６月 ２ Ｏ ｌ ５ １．０ Ｏ・５ ０・Ｏ —Ｏ． 图７伪逆算法得到的碰撞路径 Ｆｉｇ．７ Ｃｏｌｌｉｓｉｏｎ ｐａｔｈ ｏｂｔａｉｎｅｄ ｂｙ ｐｓｅｕｄｏ—ｉｎｖｅｒｓｅ ａｌｇｏｒｉｔｈｍ 图８本文算法平滑前的无碰撞路径 Ｆｉｇ．８ Ｃｏｌｌｉｓｉｏｎ—ｆｒｅｅ ｐａｔｈ ｂｅｆｏｒｅ ｂｅｉｎｇ ｓｍｏｏｔｈｅｄ ｏｂｔａｉｎｅｄ ｂｙ ｏｕｒ ａｌｇｏｒｉｔｈｍ ｌ ｒ ｌ ０Ｉ ’ ５ Ｉ｝ ｌ １ ’ ：： ｝一一 ：ｌ ｝一 一一． ｌ１ Ｊ一 ～ ： ； ＇ Ｖ 匿 一一 ： ；ｌｌ Ｉｌ 一一＇ Ｉ ｉ… ＿＿—＿—－ —— ４ １一一 一 Ｌ －！ｉ ｝ ． ． １ 下¨ ０ 图９本文算法平滑前的关节角速度 Ｆｉｇ．９ Ｊｏｉｎｔ ａｎｇｕｌａｒ ｖｅｌｏｃｉｔｉｅｓ ｏｆ ｔｈｅ ｍａｎｉｐｕｌａｔｏｒ ｂｙ ｏｕｒ ａｌｇｏｒｉｔｈｍ ｂｅｆｏｒｅ ｂｅｉｎｇ ｓｍｏｏｔｈｅｄ 下面采用路径平滑算法对上面得到的折线路径进行平滑。结果发现原有的６ｏ个子运动组成的 序列只需分割成２步即可避开障碍物，第ｌ步由初始构型运动至中间构型，第２步由中间构型至目 标构型，各构型见表３所示。机械臂的运动过程见图１０所示。每一步运动时间设定为２０ ８，关节 角运动规律采用多项式，使得每一步开始和结束时，关节角速度和角加速度为０。图１１给出了各 个关节角的角速度曲线。从图１１可以看出，与图９相比，机械臂各个关节角的运动规律非常平滑， 易于实现，并且操作时间大大减小，便于对非合作目标的快速捕获。 表３平滑后路径上的３个关键构型 Ｔａｂ．３ Ｔｈｒｅｅ ｋｅｙ ｃｏｎｆｉｇｕｒａｔｉｏｎｓ ｏｎ ｔｈｅ 初始构型 中间构型 一Ｏ．５１０９ 目标构型 一Ｏ．Ｏ５６７ １．５７０７ ３．１４１４ １ Ｏ ４．６１２７ １．４５６９ 一２．１６９１ ４．６１２７ １．４５６９ —２．１６９１ Ｏ Ｏ Ｏ Ｏ．９５６１ —Ｏ．２７２２ Ｏ Ｏ．９５６１ ——０．３０２４ Ｏ 图１Ｏ本文算法平滑后的无碰撞路径 Ｆｉｇ．１０ Ｃｏｌｌｉｓｉｏｎ—ｆｒｅｅ ｐａｔｈ ａｆｔｅｒ ｂｅｉｎｇ ｓｍｏｏｔｈｅｄ ｏｂｔａｉｎｅｄ ｂｙ ｏｕｒ ａｌｇｏｒｉｔｈｍ ２０１２年６月 中国空间科学技术 ５ ’５５ 、 、、／、、、 ～ Ｓ 趟 ０ 拉 ～…～ 担 ｌ＿／ 础 啦 妞 堪 暴 ＼ ／ ＼ ／ 时间／ｓ 时间／ｓ （ａ）第１、２、３关节角速度 （ｂ）第４、５、６关节角速度 图１１本文算法平滑后的关节角速度 Ｆｉｇ．１ １ Ｊｏｉｎｔ ａｎｇｕｌａｒ ｖｅｌｏｃｉｔｉｅｓ ｏｆ ｔｈｅ ｍａｎｉｐｕｌａｔｏｒ ｂｙ ｏｕｒ ａｌｇｏｒｉｔｈｍ ａｆｔｅｒ ｂｅｉｎｇ ｓｍｏｏｔｈｅｄ ａ 以上算法在同一台计算机上运行，伪逆算法耗时７．８８ Ｓ，而在本文的算法中，路径搜索耗时 参一 ｇ ６．６８ Ｓ，路径平滑耗时６．４５ Ｓ。可见，本文算法与伪逆算法相比，可以找到一条平滑的无碰撞路径， 计算量虽然有所增加，但计算量的增加是可以接受的。 考ｍ ｏ 文 ５ 结束语 献∞ ｍ 印 针对冗余空间机械臂粗捕获段的无碰撞轨迹规划提出的试探性规划算法，与现有算法相比，不 ∞ Ｃ 需求解Ｃ空间，为航天工程应用提供了一种简单有效的方法。算例结果表明，该算法是实用和有 效的，得到的轨迹不仅能避免与障碍物相碰，还较为平滑，易于工程实现。 口 ］Ｊ 陋 Ｅ Ｅ Ｅ １］ＬＯＺＡＮＯＰＥＲＥＺ Ｔ．Ｓｐａｔｉａｌ ｐｌａｎｎｉｎｇ １９８３，３２（２）：１０８ １２Ｏ． 锄 吐 Ｓ 阳 ［２］ ＺＨＡＮＧ ＣＨＥＮＧＫｕＮ，ｓｕＮ ＨＡＮｘｕ，ＪＩＡ ＱＩＮＧｘｕＡＮ，ｅｔ ａ１．Ａ ｎｏｖｅ１ ｄｉｖｉｓｉｏｎ ｂａｓｅｄ ｓｅｌｆ－ｍｏｔｉｏｎ ａｌｇｏｒｉｔｈｍ 叫 ｆｏｒ ａｖｏｉｄｉｎｇ ｏｂｓｔａｃｌｅｓ ｆｏｒ ｒｅｄｕｎｄａｎｔ ｍａｎｉｐｕｌａｔｏｒｓ Ｉｔ］／／Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ａｕｔｏｍａｔｉｏｎ ａｎｄ Ｉ ｏｇｉｓｔｉｃｓ．Ｊｉｎａｎ：ＩＥＥＥ，２００７：８５２ ８５７． ∞ 盯 ［３］ ＺＨＡＮＧ ＹｕＮ０ＮＧ，ＷＡＮＧ ＪＵＮ．Ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ ｆｏｒ ｋｉｎｅｍａｔｉｃａｌｌｙ ｒｅｄｕｎｄａｎｔ ｍａｎｉｐｕｌａｔｏｒｓ ｕｓｉｎｇ ａ 叫 Ｂ：Ｃｙｂｅｒｄｕａｌ ｎｅｕｒａｌ ｎｅｔｗｏｒｋ［Ｊ］．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｓｙｓｔｅｍｓ，Ｍａｎ，ａｎｄ Ｃｙｂｅｒｎｅｔｉｃｓ，Ｐａｒｔｎｅｔｉｃｓ， ２００４，３４（１）：７５２—７５９． 时 Ｓ ［４］ Ｃ０ＮＫＵＲ Ｅ Ｓ，ＢＵＣＫＩＮＧＨＡＭ Ｒ，ＨＡＲＲＩＳ０Ｎ Ａ．Ｔｈｅ ｂｅａｍ ａｎａｌｙｓｉｓ ａｌｇｏｒｉｔｈｍ ｆｏｒ ｐａｔｈ ｐｌａｎｎｉｎｇ ｆｏｒ ｒｅｄｕｎｄａｎｔ ｍａｎｉｐｕｌａｔｏｒｓ［Ｊ］．Ｍｅｃｈａｔｒｏｎｉｃｓ，２００５，１５（１）：６７－９４． ａ１．Ｒｅｄｕｃｔｉｏｎ ｏｆ ｆｒｅｅ－ｓｐａｃｅ－ｌｏｓｓ ｆｏｒ ｇｏｏｄ ａｎｄ ｒａｐｉｄ Ｅｓ］ ＢＡＩ ＡＧＵＥＲ Ｃ，ＢＡＲＲＩＥＮＴＯＳ Ａ，Ｒ０ＤＲＩＧＵＥＺ Ｆ Ｊ，ｅｔ３Ｄ ｐａｔｈ ｐｌａｎｎｉｎｇ ｏｆ ６ＤＯＦ ｒｏｂｏｔｓ［Ｊ］．Ｊｏｕｒｎａｌ ｏｆ Ｉｎｔｅｌｌｉｇｅｎｔ ａｎｄ Ｒｏｂｏｔｉｃ Ｓｙｓｔｅｍｓ，１９９５，１３（３）：２６３—２７８． ［６］ ＮＥＷＭＡＮ ｗ Ｓ．Ｒｅａｌ—ｔｉｍｅ ｃｏｎｆｉｇｕｒａｔｉｏｎ ｓｐａｃｅ ｔｒａｎｓｆｏｒｍｓ ｆｏｒ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ ｒＪ＿．Ｔｈｅ Ｉｎｔｅｒｎａｔｉｏｎａ１ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｅｓ Ｒｅｓｅａｒｃｈ，ｌ９９１，１０（６）：６５０—６６７． Ｅ７］ 黄献龙，梁斌，吴宏鑫．机器人避碰规划综述ＥＪ］．航天控制，２００２，２Ｏ（１）：３４—４０，４６． ＨＵＡＮＧ ＸＩＡＮＬＯＮＧ，ＬＩＡＮＧ ＢＩＮ，ｗｕ ＨＯＮＧＸＩＮ．Ａ ｓｕｒｖｅｙ ｏｎ ｒｏｂｏｔｉｃｓ ｃｏｌｌｉｓｉｏｎ ａｖｏｉｄａｎｃｅ ｐｌａｎｎｉｎｇ Ｉ－Ｊ］． Ａｅｒｏｓｐａｃｅ Ｃｏｎｔｒｏ１，２００２，２０（１）：３４—４０，４６． ［８］ ＣＨＯＮＧ Ｊ Ｗ Ｓ，ＯＮＧ Ｓ Ｋ，ＮＥＥ Ａ Ｙ Ｃ，ｅｔ ａ１．Ｒｏｂｏｔ ｐｒｏｇｒａｍｍｉｎｇ ｕｓｉｎｇ ａｕｇｍｅｎｔｅｄ ｒｅａｌｉｔｙ：ａｎ ｉｎｔｅｒａｃｔｉｖｅ ｍｅｔｈｏｄ ｏｒｆ ｐｌａｎｎｉｎｇ ｃｏｌｌｉｓｉｏｎ－ｆｒｅｅ ｐａｔｈｓ Ｉ－ｊ］．Ｒｏｂｏｔｉｃｓ ａｎｄ Ｃｏｍｐｕｔｅｒ－Ｉｎｔｅｇｒａｔｅｄ Ｍａｎｕｆａｃｔｕｒｉｎｇ，２００９，２５（３）：６８９—７０１． 作者简介 王 悦 １９８７年生，２００９年毕业于北京航空航天大学飞行器设计专业。现为北京航空航天大学 飞行器设计专业博士研究生，研究方向为基于几何力学和动力系统理论的天体力学和航天飞行力学。 ５６ 中国空间科学技术 ２Ｏ１２年６月 Ｃｏｌｌｉｓｉｏｎ—ｆｒｅｅ Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ Ａｌｇｏｒｉｔｈｍ ｆｏｒ Ｒｅｄｕｎｄａｎｔ Ｓｐａｃｅ Ｍａｎｉｐｕｌａｔｏｒｓ ｄｕｒｉｎｇ Ｃｏａｒｓｅ Ｔａｒｇｅｔ Ｃａｐｔｕｒｉｎｇ Ｗａｎｇ Ｙｕｅ Ｊｉａ Ｙｉｎｇｈｏｎｇ Ｘｕ Ｓｈｉｊ ｉｅ （Ｓｃｈｏｏｌ ｏｆ Ａｓｔｒｏｎａｕｔｉｃｓ，Ｂｅｉｈａｎｇ Ｕｎｉｖｅｒｓｉｔｙ，Ｂｅｉｊ ｉｎｇ １ ００ １ ９ １） Ａｂｓｔｒａｃｔ Ｔｈｅ ｅｘｉｓｔｉｎｇ ｃｏｌｌｉｓｉｏｎ—ｆｒｅｅ ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ ａｌｇｏｒｉｔｈｍｓ ｂａｓｅｄ ｏｎ ｃｏｎｆｉｇｕｒａｔｉｏｎ ｓｐａｃｅ（Ｃ—Ｓｐａｃｅ） ｎｅｅｄ ｔｏ ｆｉｎｄ ｔｈｅ ｂｏｕｎｄａｒｉｅｓ ｏｆ Ｃ—ｓｐａｃｅ ｏｂｓｔａｃｌｅｓ ｂｙ ｓｏｌｖｉｎｇ Ｃ—ｓｐａｃｅ， ｗｈｉｃｈ ｗｉｌｌ ｃｏｓｔ ｍｕｃｈ ｃｏｍｐｕｔａｔｉｏｎ ａｎｄ ｍｅｍｏｒｉｅｓ ｆｏｒ ｒｅｄｕｎｄａｎｔ ｍａｎｉｐｕｌａｔｏｒｓ ｗｉｔｈ ｌａｒｇｅｒ ｄｅｇｒｅｅｓ ｏｆ ｆｒｅｅｄｏｍ（ＤＯＦ）．Ａ ｎｏｖｅ１ ａｌｇｏｒｉｔｈｍ ｗａｓ ｐｒｏｐｏｓｅｄ ｔｏ ｆｉｎｄ ｔｈｅ ｃｏｌｌｉｓｉｏｎ—ｆｒｅｅ ｐａｔｈ ｉｎ Ｃ—ｓｐａｃｅ ｂｙ ｔｅｓｔ ｎｇ ｍｏｔｉｏｎ ｄｉｒｅｃｔｉｏｎｓ ｗｉｔｈｏｕｔ ｓｏｌｖｉｎｇ Ｃ－ｓｐａｃｅ．Ｔｈｉｓ ａｌｇｏｒｉｔｈｍ ｃｏｎｔａｉｎｅｄ ｆｏｕｒ ｓｕｌ＞ａｌｇｏｒｉｔｈｍｓ：ｔｈｅ ｃｏ１１ｉｓｉｏｎ ｄｅｔｅｃｔｉｏｎ ａｌｇｏｒｉｔｈｍ，ｔｈｅ ｃｏｌｌｉｓｉｏｎ—ｆｒｅｅ ｔａｒｇｅｔ ｃｏｎｆｉｇｕｒａｔｉｏｎ ｓｏｌｖｅｒ，ｔｈｅ ｃｏｌｌｉｓｉｏｎ—ｆｒｅｅ Ｄａｔｈ—ｓｅａｒｃｈｉｎｇ ａｌｇｏｒｉｔｈｍ ａｎｄ ｔｈｅ ｐａｔｈ—ｓｍｏｏｔｈｉｎｇ ａｌｇｏｒｉｔｈｍ． Ｇｉｖｅｎ ｔｈｅ ｄｅｓｉｒｅｄ ｐｏｓｉｔｉｏｎ ａｎｄ ａｔｔｉｔｕｄｅ ｏｆ ｔｈｅ ｅｎｄ－ｅｆｔｅｃｔｏｒ，ｔｈｅ ｔａｒｇｅｔ ｃｏｎｆｉｇｕｒａｔｉｏｎ ｓｏｌｖｅｒ ｃａｌｃｕｌａｔｅｄ ａ ｃｏｌｌｉｓｉｏｎ—ｆｒｅｅ ｔａｒｇｅｔ ｃｏｎｆｉｇｕｒａｔｉｏｎ．Ｔｈｅｎ ｔｈｅ ｐａｔｈ—ｓｅａｒｃｈｉｎｇ ａｌｇｏｒｉｔｈｍ ｆｏｕｎｄ ａ ｃｏｌｌｉｓｉｏｎ—ｆｒｅｅ ｐａｔｈ ｗｉｔｈ Ｃ—ｓｐａｃｅ ｏｂｓｔａｃ１ｅ ｂｏｕｎｄａｒｉｅｓ ｕｎｋｎｏｗｎ． Ｆｉｎａｌｌｙ，ｔｈｅ ｐａｔｈ—ｓｍｏｏｔｈｉｎｇ ａｌｇｏｒｉｔｈｍ ｓｍｏｏｔｈｅｄ ｔｈｅ ｏｂｔａｉｎｅｄ Ｄａｔｈ． Ｔｏ ｖｅｒｉｆｖ ｔｈｅ ｅｆｆｅｃｔｉｖｅｎｅｓｓ ｏｆ ｔｈｅ ａｌｇｏｒｉｔｈｍ，ｔｗｏ ｎｕｍｅｒｉｃａｌ ｅｘａｍｐｌｅｓ ｗｅｒｅ ｃａｒｒｉｅｄ ｏｕｔ・ Ａｎｄ ｔｈｅ ｒｅｓｕｉｔｓ ｉｎｄｉｃａｔｅ ｔｈａｔ ｔｈｅ ａｌｇｏｒｉｔｈｍ ｉｓ ｅｆｆｉｃｉｅｎｔ ａｎｄ ｃａｎ ｂｅ ｕｓｅｄ ｉｎ ｆａｓｔ ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ ｆｏｒ ｒｅｄｕｎｄａｎｔ ｍａｎｉｐｕｌａｔｏｒｓ ｉｎ ｓｐａｃｅ ａｐｐｌｉｃａｔｉｏｎｓ． Ｋｅｙ ｗｏｒｄｓ Ｒｅｄｕｎｄａｎｔ ｍａｎｉｐｕｌａｔｏｒｓ Ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ Ｃｏｌｌｉｓｉｏｎ ａｖｏｉｄａｎｃｅ Ｃｏｎｆｉｇｕｒａｔｉｏｎ ｓｐａｃｅ （编辑：车晓玲） Ｙ Ｕ Ｕ ｎ （上接第４８页） Ｙ ．一 Ｐｌａｎａｒ Ｓｅａｒｃｈ Ａｌｇｏｒｉｔｈｍ ｆｏｒ ＧＰＳ Ａｍｂｉｇｕｉｔｙ Ｒｅｓｏｌｕｔｉｏｎ ．ｍ ， ｗｉｔｈ Ｓｈｏｒｔ Ｂａｓｅｌｉｎｅ Ｌｅｎｇｔｈ Ｃｏｎｓｔｒａｉｎｔ Ｐａｎｇ Ｃｈｕｎｌｅｉ Ｚｈａｏ Ｘｉｕｂｉｎ Ｌｕ Ｙａｎ ｅ Ｙａｎ Ｙｕｇｕｏ Ｘ ａ ｎ ７ ｌ （Ｃｏｉｌｅｇｅ ｏｆ Ｔｅｌｅｃｏｍｍｕｎｉｃａｔｉｏｎ Ｅｎｇｉｎｅｅｒｉｎｇ，Ａｉｒ Ｆｏｒｃｅ Ｅｎｇｉｎｅｅｒｉｎｇ Ｏ Ｏ ７ ７ ＼， Ａｂｓｔｒａｃｔ Ａ ｎｅｗ ｐ１ａｎａｒ ｓｅａｒｃｈ ｍｅｔｈｏｄ ｆｏｒ ＧＰＳ ａｍｂｉｇｕｉｔｙ ｒｅｓｏｌｕｔｉｏｎ ｗｉｔｈ ｓｈｏｒｔ ｂａｓｅｌｉｎｅ ｌｅｎｇｔｈ ｃｏｎｓｔｒａｉｎｔｓ ｗａｓ ｐｒｏｐｏｓｅｄ．Ｔｈｅ ｉｎｔｅｇｅｒ ａｍｂｉｇｕｉｔｙ ｗａｓ ｆｉｘｅｄ ｉｎ ｔｈｅ ｃｏｎｄｉｔｉｏｎ ｏｆ ｌｅａｓｔ ｓｑｕａｒｅ ｅｒｒｏｒ ｂｙ ｓｅａｒｃｈｉｎｇ ｔｈｅ ｂａｓｅｌｉｎｅ ｅｌｅｖａｔｉｏｎ ａｎｄ ａｚｉｍｕｔｈ ａｎｇｌｅ．Ｔｈｅ ｍｏｄｅｌ ｗａｓ ｅｓｔａｂｌｉｓｈｅｄ， ｔｈｅ ｓｅａｒｃｈ ｐｒｉｎｃｉｐｌｅ ａｎｄ ａｐｐｌｉｃａｔｉｏｎ ｐｒｏｃｅｓｓ ｏｆ ｔｈｅ ｎｅｗ ａｌｇｏｒｉｔｈｍ ｗｅｒｅ ｄｉｓｃｕｓｓｅｄ， ａｎｄ ｔｈｅ ｓｔｅｐ ｏｆ ｅｘｔｅｎｓｉｖｅ ｓｅａｒｃｈ ａｎｄ ｉｎｔｅｎｓｉｖｅ ｓｅａｒｃｈ ｗａｓ ｉｎｄｕｃｅｄ．Ｔｈｒｏｕｇｈ ｔｈｅ ｅｘｐｅｒｉｍｅｎｔｓ ａｎｄ ｃｏｍｐａｒｉｎｇ ｗｉｔｈ ＬＡＭＢＤＡ ａｌｇｏｒｉｔｈｍ，ｔｈｅ ｎｅｗ ｍｅｔｈｏｄ ｗｉｔｈ ｓｉｍｐｌｅ ａｌｇｏｒｉｔｈｍ ａｎｄ ｈｉｇｈ ｅｆｆｉｃｉｅｎｃｙ ｗａｓ Ｄｒｏｖｅｄ． Ｔｈｅ ｅｒｒｏｒ ｉｓ ｎｏ ｍｏｒｅ ｔｈａｎ ｌ ｃｍ ｉｎ ｂａｓｅｌｉｎｅ ｐｒｅｃｉｓｉｏｎ， ｎｏ ｍｏｒｅ ｔｈａｎ ０．６。 ｉｎ ｅｌｅｖａｔｉｏｎ Ｄｒｅｃｉｓｉｏｎ ａｎｄ ｎｏ ｍｏｒｅ ｔｈａｎ ０．４。ｉｎ ａｚｉｍｕｔｈ ｐｒｅｃｉｓｉｏｎ．Ｔｈｉｓ ｍｅｔｈｏｄ ｉｓ ａｌｓｏ ａｄａｐｔｉｖｅ ｔｏ ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ． Ｋｅｙ ｗｏｒｄｓ Ｓｈｏｒｔ ｂａｓｅｌｉｎｅ ｃｏｎｆｉｎｅｍｅｎｔ Ｔｗｏ ｄｉｍｅｎｓｉｏｎａｌ ｓｅａｒｃｈｉｎｇ Ｉｎｔｅｇｅｒ ａｍｂｉｇｕｉｔｙ Ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ Ｓｐａｃｅｃｒａｆｔ （编辑：杨婵）
