考虑冗余机械臂末端运动特性的规划方法

ＩＳＳＮ　１０００－００５４　清华大学学报（自然科学版）　２０１４年第５４卷第１２期　Ｊ　Ｔｓｉｎｇｈｕａ　Ｕｎｉｖ（Ｓｃｉ＆Ｔｅｅｈｎｏ１），２０１４，Ｖｏ１．５４，Ｎｏ．１２　７／２０　１５４４　１５４８　ＣＮ　１Ｉ－２２２３／Ｎ　考虑冗余机械臂末端运动特性的规划方法　黄文炳，　孙富春，　刘华平　（清华大学计算机科学与技术系，智能技术与系统国家重点实验室，北京１０００８４）　摘要：机械臂运动规划是机器人研究领域的重点，对机械　臂能否顺利执行任务非常重要。目前，机械臂运动规划多使　用ＲＲＴ法，然而该方法是在关节空间进行规划，无法适用于　机械臂末端执行器存在约束的任务。为了克服这个不足，该　文提出了一种任务自由子空间ＲＲＴ（ｒａｐｉｄｌｙ－ｅｘｐｌｏｒｉｎｇ　ｒａｎ—　ｄｏｍ　ｔｒｅｅ）法，在末端执行器任务空间的自由子空间中构建　ＲＲＴ，并对其每步扩张进行逆运动学轨迹优化，求解出相应　的关节轨迹。此外，由于末端执行器速度对逆运动规划有重　要影响，该文在逆运动轨迹优化阶段采用了最似梯度法，不　仅考虑了末端执行器的运动速度，而且通过极小关节自由速　度和优化目标负梯度的距离，重新确定了关节自由速度，增　强了算法的优化能力。实验结果表明：该算法能有效解决　机械臂末端存在约束的问题。　关键词：冗余机械臂；运动规划；ＲＲＴ（ｒａｐｉｄｌｙ－ｅｘｐｌｏｒｉｎｇ　ｒａｎｄｏｍ　ｔｒｅｅ）法；最似梯度法　中图分类号：ＴＰ　３９１．４　文献标志码：Ａ　文章编号：ｉ０００—００５４（２Ｏ１４）１２—１５４４—０５　Ｐｌａｎｎｉｎｇ　ｍｅｔｈｏｄ　ｃｏｎｓｉｄｅｒｉｎｇ　ｔｈｅ　ｋｉｎｅｔｉｃ　ｃｈａｒａｃｔｅｒｉｓｔｉｃｓ　ｏｆ　ｔｈｅ　ｅｎｄ　ｅｆｆｅｃｔｏｒ　ｏｆ　ａ　ｒｅｄｕｎｄａｎｔ　ｍａｎｉｐｕｌａｔｏｒ　ＨＵＡＮＧ　Ｗｅｎｂｉｎｇ·ＳＵＮ　Ｆｕｃｈｕｎ，ＬＩＵ　Ｈｕａｐｉｎｇ　（Ｓｔａｔｅ　Ｋｅｙ　ｌａｂｏｒａｔｏｒｙ　ｏｆ　Ｉｎｔｅｌｌｉｇｅｎｔ　Ｔｅｃｈｎｏｌｏｇｙ　ａｎｄ　Ｓｙｓｔｅｍｓ，　Ｄｅｐａｒｔｍｅｎｔ　ｏｆ　Ｃｏｍｐｕｔｅｒ　Ｓｃｉｅｎｃｅ　ａｎｄ　Ｔｅｃｈｎｏｌｏｇｙ，　Ｔｓｉｎｇｈｕａ　Ｕｎｉｖｅｒｓｉｔｙ，Ｂｅｉｊｉｎｇ　１０００８４，Ｃｈｉｎａ）　ｅｎｄ—ｅ｛ｆｅｃｔｏｒ－ｃｏｎｓｔｒａｉｎｅｄ　ｔａｓｋｓ．　Ｋｅｙ　ｗｏｒｄｓ：ｒｅｄｕｎｄａｎｔ　ｍａｎｉｐｕｌａｔｏｒ；　ｋｉｎｅｍａｔｉｃｓ　ｐｌａｎｎｉｎｇ　ｒａｐｉｄｌｙ—ｅｘｐｌｏｒｉｎｇ　ｒａｎｄｏｍ　ｔｒｅｅ　（ＲＲＴ）　ｍｏｓｔ—　ｌｉｋｅ—－ｇｒａｄｉｅｎｔ　ｍｅｔｈｏｄ　ＲＲＴ（ｒａｐｉｄｌｙ—ｅｘｐｌｏｒｉｎｇ　ｒａｎｄｏｍ　ｔｒｅｅ）法［　］是一　种在关节空间直接规划机械臂运动的算法。该方法　按照预先设定的启发准则在机械臂关节空间不断扩　张，在满足不完整约束的同时能快速得到一条无障　碍碰撞轨迹，不但计算快速高效，而且是概率完备　的。为了提高收敛速度，文［２］提出了ＲＲＴ—Ｃｏｎ—　ｎｅｃｔ法。文［３］发现尽管ＲＲＴ以概率为１收敛到　可行解，但是收敛到最优解的概率为零，因此提出了　ＲＲＴ　法，并证明在一定条件下，该方法收敛到最优　解的概率为１。随后，文Ｃ４］在ＲＲＴ　法基础上构建　双向树，提高了ＲＲＴ　法的计算效率。ＲＲＴ法自提　出以来就得到了很多研究者的关注，被广泛应用到　宇航机器人的运动规划、移动机器人导航和直升机　导航等领域中＿５嵋］，是目前为止机器人轨迹优化领　域应用范围最广的方法之一。　然而，在实际作业中，因为实验环境和任务类型　等的限制，机器人末端执行器往往会存在运动约束。　传统关节空间ＲＲＴ法是在关节空间对机械臂规　划，没有考虑末端执行器的运动轨迹，无法适用于机　械臂末端存在运动限制的规划任务。但如果在末端　执行器不受运动约束的运动变量所组成的自由子空　间进行ＲＲＴ规划，规划的末端轨迹自然就能满足　任务的运动约束。另外，为了求解出相应的关节轨　迹，需要进行逆运动轨迹优化。逆运动轨迹优化的　收稿日期：　２Ｏ１４—０１－２３　基金项目：　国家自然科学资金国际合作重大项目（６１２１００１３）　作者简介：　黄文炳（１９８８一），男（汉），广东，博士研究生。　通信作者：　孙富春，教授，Ｅ—ｍａｉｌ：ｆｃｓｕｎ＠ｔｓｉｎｇｈｕａ．ｅｄｕ．ｃｎ　黄文炳，等：　考虑冗余机械臂末端运动特性的规划方法　传统方法主要有梯度投影（ＧＰ）法【９］、加权最小范　数（ｗＬＮ）法　。。和扩展Ｊａｃｏｂｉ法口　等。这些方法均　没有考虑末端执行器的速度，限制了逆运动学轨迹　优化的性能。　为此，本文提出了自由子空间法，即在自由子空　间构建末端轨迹的ＲＲＴ，然后通过逆运动学轨迹规　划反求出相应的关节轨迹。不但保留了ＲＲＴ法规　划快速的优点，而且得到的关节轨迹能满足末端的　运动约束。同时，对于逆运动规划，本文还提出了一　种考虑了末端运动速度的方法——最似梯度（ｍｏｓｔ—　ｌｉｋｅ—ｇｒａｄｉｅｎｔ，ＭＬＧ）法，提高了逆运动规划算法的　优化能力。　１工作自由子空间的ＲＲＴ轨迹规划　基本ＲＲＴ法的规划步骤详见文Ｅ３－１，包含了２　个基本的算法１和２，分别如图１和２所示。算法１　和２中，Ｖ是ＲＲＴ所有连线组成的集合，Ｅ是　ＲＲＴ所有节点组成的集合，ｑｉｎｉｔ代表初始关节角，声　代表空集。图３中，ＲＲＴ法是在关节空间进行的，　在每步迭代中根据ｓａｍｐｌｅ函数生成１个随机位姿　口…　，然后再从已生成的树Ｔ中找出在距离函数　ｄｉｓｔ意义下离口…ａ最近的１个点ｑ　，根据这２个点　由函数ｓｔｅｅｒ生成新的点口…，并判断这个点是否碰　到障碍物，如果没碰到，就把这个点ｑ　以及连线　（ｇ　，ｑ　）加到树Ｔ中，形成了新的树。重复迭代　直到生成的树中至少有１个点ｑ离目标点ｑ　。的距　离充分小。　图１算法１　图２算法２　＿ｌ、　，，：：：≥　。　：　。－－－。　：　图３　ＲＲＴ生长过程　自由子空间法（简称为算法３）是在基本ＲＲＴ　法上改进得到的（见图４）。设机械臂末端所有变量　组成的空间为ｘ，末端受到约束的变量组成的空间　为ｘ　，则ｘ　为ｘ的子集。令ｘ２一ｘ—ｘ　，称ｘ　为　自由子空间。自由子空间ＲＲＴ法是在ｘ。中构建　ＲＲＴ，然后对ＲＲＴ每一迭代进行逆运动规划求解　相应的关节轨迹。自由子空间的逆运动规划采用　ＭＬＧ法。　算法３中Ｘ　表示给定轨迹的末端执行器的位　姿，Ｘ　表示需要规划的末端位姿。由于ＲＲＴ是在　ｘ　进行的，因此第１行将ｘ　的初始值　赋予　ＲＲＴ树。第５行由Ｅｘｔｅｎｄ函数返回最近邻近点　ｚ　以及其对应的关节变量ｑ　和时间ｔ　，同时　还返回新扩展的点Ｘ…及其时间ｔ…。第６、７行Ｘ。　和Ｘ　分别表示最近邻近时间节点和新的时间节点　上的末端位姿。第８行给定末端运动轨迹为一直线　（（　。，ｔ　）为起点，（　，ｔ…）为终点），应用ＭＬＧ法　对机械臂进行逆运动学轨迹优化。由于ＲＲＴ规划　是在ｘｚ进行的，因此除了障碍物检测，还要进行边　界检测，即判断ＢｏｕｎｄＦｒｅｅ（ｑ…）是否为１。　图４算法３　２最似梯度法　冗余机械臂逆运动轨迹优化的求解方法通常　采取以下局部方法口　：给定关节的初始位姿口。和　末端执行器运动路径ｘ（ｓ），求解关节自由速度　（ｔ　）和末端执行器沿着既定路径的速度Ｓ　（ｔ　），使　得机械臂满足边界约束，且极小关于ｑ（ｔ　）的指　清华大学学报（自然科学版）　标Ｈ（口（ｔ…））：　Ｍｉｎ　Ｈ（ｑ（　斗１）），　ｓ．ｔ．口　（　）一Ｊ　ＴｄＸｓ（￡　）＋（Ｊ～Ｊ　Ｊ）＇，（ｔｉ），　ｑ（ｔｉ＋１）一ｑ（ｔ　）＋ｑ　（　）　（１）　其中ｒ一￡…一ｆ　为时间步长。此外，口（￡）和Ｓ　（　）存　在边界约束：ｑｍｉｎ＜ｑ（　）＜ｇ　，Ｓ　（￡）＞０，Ｓ　（０）一０，　Ｓ　（Ｔ）一０。　传统优化方法如ＧＰ法、ＷＬＮ法和扩展Ｊａｃｏｂｉ　法　。　等均取定Ｓ　（　）一１，然后选取合适的ｖ（ｔ　）使　得目标最优。其中，ＧＰ法是将目标函数Ｈ（ｑ）的梯　度向量ＶＨ（ｑ）乘以系数忌作为＇，（ｔ　）。相比传统优　化方法，本文进行了以下改进：　１）重新确定ｖ（ｔ　）值，使得关节变量的迭代过　程近似为最速下降过程，实现目标的快速优化，提高　了算法的优化速度；　２）在只给定末端执行器的运动路径情况下，将　末端执行器沿着这条既定的路径的运动速度作为一　个优化变量，增强了算法的优化能力。　令ｙ　一‘，＋　Ｏ，Ａ　一（Ｊ—Ｊ＋ｔ，），式（１）的约束简　Ｓ　记为　口：一ｙ　Ｓ；＋Ａ　１，　，　ｑ斗１一口　＋ｑ　ｒ．　（２）　在最速下降法中，目标函数优化最快的方向是其负　梯度方向。因此，对于式（２），考虑让迭代过程中迭　代方向ｑ　充分接近一ｋ　ＶＨ（口　）即Ｈ（ｑ…）尽量位　于Ｈ（ｑ　）的最速下降方向上，这样能快速极小Ｈ　（ｑ　＋１）。　下面用２范数刻画ｑ　与一ｋ　ＶＨ（ｑ　）的接近程　度。考虑优化问题：　Ｍｉｎ　ｌ　ｌｑ　＋ｋ　ＶＨ（ｑ　）ｌ　，　ｓ．ｔ．　口　一Ｙ　Ｓ　＋Ａ　＇，　，　ｑｉ＋１一ｑ　＋ｑ　ｒ．　（３）　其极小点为　丢愚［　箍Ｈ　㈩　其中：　Ａｉ　Ｅｏ，Ｂ　］Ｑ　，ｌ　Ｉ—Ｑ　１，　，Ｈ　∈　ｍ，　∈　Ｂ　∈　一　且列满秩，Ｑ　为列变换方阵。　则有　ｑ；一ｙ　Ｓ　＋Ｂ　Ｈ　．　（５）　为了防止机械臂的末端执行器运动速度太小，　要限定Ｓ；的下限：Ｓ：＞Ｓ　。　＞０。同时，为了系统稳　定，需要对ｑ　和ｑ　进行边界约束，可以通过调节ｋ　避免ｑ　和ｑ：溢出边界。　３实验与分析　３．１最似梯度法仿真实验　在实际任务中，机械臂的每个关节都是有一定　的活动范围。为了使关节变量ｑ限定在活动范围　内，文［１３］提出了式（６）的罚函数，以避免关节变量　达到关节极限。　Ｈ（ｇ　１　（６）　当关节变量趋近关节极限时，Ｈ（ｑ）趋近无穷　大；当关节变量趋近关节极限的中间值时，Ｈ（口）趋　近最小值。因此，把Ｈ（ｑ）作为逆运动轨迹优化目　标，通过极小Ｈ（ｑ）就能使得关节变量远离关节　极限。　为了验证ＭＬＧ法的效果，本文使７自由度的　雄克（ＳＣＨＵＮＫ）机械臂（见图５）的末端沿直线运　动，然后对各关节运动轨迹进行数值计算，并把结果　与ＧＰ法和ＷＬＮ法的进行对比。　图５　７自由度机械臂　图６是ＭＬＧ法、ＧＰ法和ＷＬＮ法这３种方法　得到的目标函数Ｈ（ｑ）的对比图。　３Ｏ　２０　１Ｏ　Ｕ　２０　４０　６０　８０　ｌ【ｊＩ）　ｌ２１）　迭代步数　图６逆运动优化　从图６中可以发现ＭＬＧ法能快速到达最小　值，并且随着迭代步数的增加能稳定地维持在最小　值附近。ＧＰ法和ｗＬＮ法的优化效果不如ＭＬＧ　黄文炳，等：　考虑冗余机械臂末端运动特性的规划方法　法。对于ＧＰ法而言，随着迭代步数的增加，目标值　出现了反弹。综上，本文的ＭＬＧ法明显优于传统　方法。　３．２　自由子空间ＲＲＴ法机器人平台实验　３．２．１　实验说明　实验任务是机械臂从位置Ａ抓取一个装有水　的水杯到达位置Ｂ，保证在运动的过程中杯中的水　没有溢出，并且能避开障碍物（见图７）。本实验中，　长度的单位均为ｍ，角度的单位均为ｒａｄ。　图７抓取水杯买验　规划空间为末端自由变量组成的子空间。对于　个刚体，空间坐标ｘ为６维，前３个分量为位置　变量３２、Ｙ和ｚ，后３个为Ｅｕｌｅｒ角　、口和），。在本　例中，为了使得水杯的水不能溢出，则必须保持口和　），不变，则自由子空间自由度只有４个：．ｚ、　和　ａ。另外为了提高ＲＲＴ规划的收敛速度，参考文　ＥｌＯ］的做法，在ＲＲＴ每规划出新的一点之后，都进　行一次逆运动规划，使得机械臂能从该点直线运动　到目标点。如果逆运动规划成功，则停止ＲＲＴ规　划，否则继续扩张ＲＲＴ。　３．２．２实验参数　１）初始位姿。　初始关节角可以任意选择，本实验考虑到实验　环境的限制，故选择了一个比较合适的关节角：　ｑ。一［｛，詈，０，詈，０，詈，０］．　根据ｑ。，可以求得机械臂末端初始坐标：　ｘ。一ＬＯ．５０５，０．５０５，０．７２２，２．３５６，１．５７１，４．７１２］．　２）目标点。　末端目标位置为Ｘ　ｌ一（０．５，一０．３，０．５），　末端目标位姿为　厂Ｏ　０·６　０·８］　Ｒ　一１ｌ　０　０．８　—０．６　ｌ，　１　ｏ　ｏ　Ｉ　１５４７　则目标坐标为　ｘ　ｌ—Ｅｏ．５，一０．３，０．５０．９２７，１．５７１，４．７１２］．　３）末端限制。　△　：△ｙ一０即保证　和），不变，末端自由变量　为（ｚ，ｙ，　，ａ）。　４）障碍物。　障碍物为正方形，其中心为（０．４，０．３，ｏ．５），边　长为０．２。　３．２．３实验结果　参照文Ｅｌ４］的方法，机械臂最终的轨迹是由２　部分组成，一部分是ＲＲＴ规划轨迹从初始点规划　到中间点（见图８ａ），另一部分是从中间点到目标点　的直线轨迹（见图８ｂ）。从图８ａ中可以发现，由于　存在障碍物，如果没有ＲＲＴ规划，从初始点是无法　直线规划到目标点的。而经过若干步ＲＲＴ规划　后，机械臂就可以先规划到中间点，然后再直线规划　到目标点，如图８ｂ所示。算法最终的规划目标坐标　为ｘ‰　ｌ一［０．４９８，一０．３０６，０．４９９，０．９２１，１．５７２，　７．７２６］，与预定结果之间的误差为　ｌ　ｌｘ‰　ｉ—ｘ　ｌ　ｌ　ｌ２—０．０１６．　可见误差是很小的。从图８ｃ可以发现口和ｙ几乎　没有变化，这说明规划算法能保证实验中水杯的　水不会溢出。从图８ｄ发现算法规划出的关节轨　迹光滑，这说明了机械臂在运动过程中没有出现　震荡。　０．８０　ｇ　０．７０　Ｏ．６０　Ｏ．５０　０　６０　Ｏ．５２　（ａ）ＲＲＴ规划　Ｏ·８Ｏ　初始点　Ｏ·７Ｏ　ｇ　０．６０　０　５０　Ｏ．６０　０．５２　（ｂ）最终规划路径　１５４８　清华大学学报（自然科学版）　２Ｏ１４，５４（１２）　｛　矗　山　迭代步数　（ｃ）Ｅｕｌｅｒ角变化　拉　迭代步数　（ｄ）关节轨迹　图８实验结果　４结论　针对机械臂末端存在运动约束的任务，本文提　出了考虑机械臂末端运动特性的规划方法。其中：　任务自由子空间ＲＲＴ法将机械臂末端执行器的轨　迹规划与逆运动学轨迹优化结合，有效解决了末端　执行器存在运动约束的难题；ＭＬＧ法不仅将末端　执行器的运动速度作为逆运动轨迹优化的变量，而　且通过极小关节自由速度与优化目标负梯度的距离　重新确定了关节自由速度，增强了逆运动轨迹规划　算法的优化能力。　本文只考虑了机械臂运动学规划，并没有考虑　机械臂的加速度和关节力矩等动力学因素，下一步　将研究如何把考虑末端特性的优化方法应用到机械　臂动力学规划中。　参考文献　（Ｒｅｆｅｒｅｎｃｅｓ）　［１］　ＬａＶａｌｌｅ　Ｓ　Ｍ，Ｋｕｆｆｎｅｒ　Ｊ　Ｊ．Ｒａｎｄｏｍｉｚｅｄ　ｋｉｎｏｄｙｎａｍｉｃ　ｐｌａｎｎｉｎｇ　［Ｊ］．Ｔｈｅ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｊｏｕｒｎａｌ　ｏｆ　Ｒｏｂｏｔｉｃｓ　Ｒｅｓｅａｒｃｈ，２００１，　２０（５）：３７８—４００．　［２］　Ｋｕｆｆｎｅｒ　Ｊ　Ｊ，ＬａＶａｌｌｅ　Ｓ　Ｍ．ＲＲＴ—ｃｏｎｎｅｃｔ：Ａｎ　ｅｆｆｉｃｉｅｎｔ　ａｐｐｒｏａｃｈ　ｔｏ　ｓｉｎｇｌｅ—ｑｕｅｒｙ　ｐａｔｈ　ｐｌａｎｎｉｎｇ［ｃ］／／ＩＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａ１　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ．　Ｓａｎ　Ｆｒａｎｃｉｓｃｏ，ＣＡ，ＵＳＡ：ＩＥＥＥ，２０００：９９５—１００１．　［３］Ｋａｒａｍａｎ　Ｓ，Ｆｒａｚｚｏｌｉ　Ｅ．Ｏｐｔｉｍａｌ　ｋｉｎｏｄｙｎａｍｉｃ　ｍｏｔｉｏｎ　ｐｌａｎｎｉｎｇ　ｕｓｉｎｇ　ｉｎｃｒｅｍｅｎｔａｌ　ｓａｍｐｌｉｎｇ—ｂａｓｅｄ　ｍｅｔｈｏｄｓ［ｃ］／／　４９ｔｈ　ＩＥＥＥ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｄｅｃｉｓｉｏｎ　ａｎｄ　Ｃｏｎｔｒｏｌ（ＣＤＣ）．　Ａｔｌａｎｔａ，Ｇｅｏｒｇｉａ，ＵＳＡ：ＩＥＥＥ，２０１０：７６８１—７６８７．　［４］Ｊｏｒｄａｎ　Ｍ，Ｐｅｒｅｚ　Ａ．Ｏｐｔｉｍａｌ　Ｂｉｄｉｒｅｃｔｉｏｎａｌ　Ｒａｐｉｄｌｙ—Ｅｘｐｌｏｒｉｎｇ　Ｒａｎｄｏｍ　Ｔｒｅｅｓ，ＭＩＴ—ＣＳＡＩＬ—ＴＲ一２０１３－０２１［Ｒ］．Ｃａｍｂｒｉｄｇｅ，　ＵＫ：Ｃｏｍｐｕｔｅｒ　Ｓｃｉｅｎｃｅ　ａｎｄ　Ａｒｔｉｆｉｃｉａｌ　Ｉｎｔｅｌｌｉｇｅｎｃｅ　Ｌａｂｏｒａｔｏｒｙ．　２０１３．　Ｅｓ］Ｆｕｌｇｅｎｚｉ　Ｃ，Ｔａｙ　Ｃ，Ｓｐａｌａｎｚａｎｉ　Ａ，ｅｔ　ａ１．Ｐｒｏｂａｂｉｌｉｓｔｉｃ　ｎａｖｉｇａｔｉｏｎ　ｉｎ　ｄｙｎａｍｉｃ　ｅｎｖｉｒｏｎｍｅｎｔ　ｕｓｉｎｇ　ｒａｐｉｄｌｙ—ｅｘｐｌｏｒｉｎｇ　ｒａｎｄｏｍ　ｔｒｅｅｓ　ａｎｄ　ｇａｕｓｓｉａｎ　ｐｒｏｃｅｓｓｅｓ［ｃ］／／ＩＥＥＥ／ＲＳＪ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｉｎｔｅｌｌｉｇｅｎｔ　Ｒｏｂｏｔｓ　ａｎｄ　Ｓｙｓｔｅｍｓ（ＩＲＯ￥２００８）．Ｎｉｃｅ，Ｆｒａｎｃｅ：ＩＥＥＥ，２００８：１０５６—　１Ｏ６２．　［６］Ｂｒｙ　Ａ，Ｒｏｙ　Ｎ．Ｒａｐｉｄｌｙ—ｅｘｐｌｏｒｉｎｇ　ｒａｎｄｏｍ　ｂｅｌｉｅｆ　ｔｒｅｅｓ　ｆｏｒ　ｍｏｔｉｏｎ　ｐｌａｎｎｉｎｇ　ｕｎｄｅｒ　ｕｎｃｅｒｔａｉｎｔｙ［ｃ］／／ＩＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ．Ｓｈａｎｇｈａｉ，Ｃｈｉｎａ：　ＩＥＥＥ，２０１１：７２３—７３０．　［７］　Ｋｏｔｂａｒｉ　Ｍ，Ｐｏｓｔｌｅｔｈｗａｉｔｅ　Ｉ．　Ａ　ｐｒ。ｂａｂｉｌｉｓｔｉｃａｌｌｙ　ｒｏｂｕｓｔ　ｐａｔｈ　ｐｌａｎｎｉｎｇ　ａｌｇｏｒｉｔｈｍ　ｆｏｒ　ＵＡＶｓ　ｕｓｉｎｇ　ｒａｐｉｄｌｙ—ｅｘｐｌｏｒｉｎｇ　ｒａｎｄｏｍ　ｔｒｅｅｓ　ＩＪ］．Ｊｏｕｒｎａｌ　ｏｆ　Ｉｎｔｅｌｌｉｇｅｎｔ＆Ｒｏｂｏｔｉｃ　Ｓｙｓｔｅｍｓ，２０１３，　７１（２）：２３１—２５３．　［８］Ｄｅｎｎｙ　Ｊ，Ｇｒｅｃｏ　Ｅ，Ｔｈｏｍａｓ　Ｓ，ｅｔ　ａ１．ＭＡＲＲＴ：Ｍｅｄｉａｌ　ａｘｉｓ　ｂｉａｓｅｄ　ｒａｐｉｄｌｙ　ｅｘｐｌｏｒｉｎｇ　ｒａｎｄｏｍ　ｔｒｅｅｓ　［ｃ］／／　ＩＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ．Ｈｏｎｇ　Ｋｏｎｇ，Ｃｈｉｎａ：ＩＥＥＥ，２０１４：９０—９７．　Ｌ９］Ｌｉｅｇｅｏｉｓ　Ａ．　Ａｕｔｏｍａｔｉｃ　ｓｕｐｅｒｖｉｓｏｒｙ　ｃｏｎｔｒｏｌ　ｏｆ　ｔｈｅ　ｃｏｎｆｉｇｕｒａｔｉｏｎ　ａｎｄ　ｂｅｈａｖｉｏｒ　ｏｆ　ｍｕｈｉｂｏｄｙ　ｍｅｃｈａｎｉｓｍｓ［Ｊ］．　Ⅱ　ＥＥ　Ｔｒａｎｓａｃｔｉｏｎｓ　ｏｎ　Ｓｙｓｔｅｍｓ，Ｍａｎ，ａｎｄ　Ｃｙｂｅｒｎｅｔｉｃｓ，　１９７７，７（１２）：８６８—８７１．　［１ｏ］Ｗｈｉｔｎｅｙ　Ｄ　Ｅ．Ｔｈｅ　ｍａｔｈｅｍａｔｉｃｓ　ｏｆ　ｃｏｏｒｄｉｎａｔｅｄ　ｃｏｎｔｒｏｌ　ｏｆ　ｐｒｏｓｔｈｅｔｉｃ　ａｒｍｓ　ａｎｄ　ｍａｎｉｐｕｌａｔｏｒｓ［Ｊ］．Ｊｏｕｒｎａｌ　ｏｆ　Ｄｙｎａｍｉｃ　Ｓｙｓｔｅｍｓ，Ｍｅａｓｕｒｅｍｅｎｔ，ａｎｄ　Ｃｏｎｔｒｏｌ，１９７２，９４（４）：３０３—　３Ｏ９．　［１１］Ｂａｉｌｌｉｅｕｌ　Ｊ．Ｋｉｎｅｍａｔｉｃ　ｐｒｏｇｒａｍｍｉｎｇ　ａｌｔｅｒｎａｔｉｖｅｓ　ｆｏｒ　ｒｅｄｕｎｄａｎｔ　ｍａｎｉｐｕｌａｔｏｒｓ［ｃ］／／ＩＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ．Ｌｏｕｉｓ，Ｍｉｓｓｏｕｒｉ，ＵＳＡ：ＩＥＥＥ，　１９８５：７２２—７２８．　［１２］陆震．冗余自由度机器人原理及应用［Ｍ］．北京：机械工业　出版社，２００６．　ＬＵ　Ｚｈｅｎ．Ｐｒｉｎｃｉｐｌｅ　ａｎｄ　Ａｐｐｌｉｃａｔｉｏｎ　ｏｆ　Ｒｅｄｕｎｄａｎｔ　Ｒｏｂｏｔ　［Ｍ］．Ｂｅｉｊｉｎｇ：Ｃｈｉｎａ　Ｍａｃｈｉｎｅ　Ｐｒｅｓｓ，２００６．（ｉｎ　Ｃｈｉｎｅｓｅ）　［１３］Ｚｇｈａｌ　Ｈ，Ｄｕｂｅｙ　Ｒ　Ｖ，Ｅｕｌｅｒ　Ｊ　Ａ．Ｅｆｆｉｃｉｅｎｔ　ｇｒａｄｉｅｎｔ　ｐｒｏｊｅｃｔｉｏｎ　ｏｐｔｉｍｉｚａｔｉｏｎ　ｆｏｒ　ｍａｎｉｐｕｌａｔｏｒｓ　ｗｉｔｈ　ｍｕｌｔｉｐｌｅ　ｄｅｇｒｅｅｓ　ｏｆ　ｒｅｄｕｎｄａｎｃｙ［ｃ］／／ＩＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ．Ｃｉｎｃｉｎｎａｔｉ，Ｏｈｉｏ，ＵＳＡ：ＩＥＥＥ，　１９９Ｏ：１００６—１０１１．　ｒ１　］　Ｔｏｕｓｓａｉｎｔ　Ｍ，Ｐｌａｔｈ　Ｎ，Ｌａｎｇ　Ｔ，ｅｔ　ａ１．Ｉｎｔｅｇｒａｔｅｄ　ｍｏｔｏｒ　ｃｏｎｔｒｏ１，ｐｌａｎｎｉｎｇ，ｇｒａｓｐｉｎｇ　ａｎｄ　ｈｉｇｈ—ｌｅｖｅｌ　ｒｅａｓｏｎｉｎｇ　ｉｎ　ａ　ｂｌｏｃｋｓ　ｗｏｒｌｄ　ｕｓｉｎｇ　ｐｒｏｂａｈｉｌｉｓｔｉｃ　ｉｎｆｅｒｅｎｃｅ　ＥＣ］／／ＩＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ．　Ａｎｃｈｏｒａｇｅ，Ａｌａｓｋａ，ＵＳＡ：ＩＥＥＥ，２０１０：３８５—３９１．