广州登高车出租,   广州登高维修车出租, 广州登高安装车出租    基于快速搜索随机树的动态避障规划

           广州登高车出租,   广州登高维修车出租,  广州登高安装车出租     基于快速搜索随机树的动态避障规划            将ＲＲＴ＊算法用于高自由度机械臂动态避障规划极具挑战。为解决前文提到的问题，以ＲＲＴ＊和ＲＴ－ＲＲＴ＊算法为基础实现适用于机械臂的快速搜索随机树动态避障规划。给出了本章所提算法系统框图，主要包括离线规划和在线规划两部分。首先给定机械臂末端目标姿态和环境地图信息，利用机械臂逆运动学求解一组自由区域的关节作为离线规划的目标节点，然后利用ＲＲＴ＊算法在给定环境下构建搜索树，将搜索到的最优路径输入给机械臂控制系统控制其运动。机械臂在运动过程中保持更新环境地图，并依此利用ＲＴ－ＲＲＴ＊算法进行在线路径规划，实时获取一条最优路径输入给机械臂运动控制系统。

          离线规划的主要任务是求解目标节点和构建搜索树。 给出了其伪码描述，本节将对其与ＲＲＴ＊算法的不同处进行详细介绍。为避免符号混淆，伪码中使用只表示机械臂末端位姿。求解关节目标；关节目标的求解对应Ａｌｇｏｒｉｔｈｍ２中第一行。当给定七自由度机械臂末端姿态时，通过逆运动学可求得无数组满足条件的关节角，介绍的基于雅可比矩阵伪逆求解法只能求得一组解，如果该解正好处于关节空间的障碍物区域将无法使用ＲＲＴ＊进行规划。为此本节采用可求得所有解的解析法进行求解。 机械臂四连杆结构简图，其中Ａ、Ｂ、Ｃ和Ｄ点分别表示机械臂关节２、４、６和７。Ａ点固连于机械臂底座，当机械臂在世界坐标系中的位置确定时，Ａ点位置也确定，姿态随关节角度变化。机械臂末端坐标系固连于Ｄ点，则可通过齐次变换关系求得Ｃ点位置，但无法确定其姿态。因此，当给定机械臂末端目标位姿后即可唯一求解Ａ、Ｃ和Ｄ点的位置，此时无论Ｂ点如何运动都不影响机械臂末端位姿。当固定Ａ、Ｃ点后，Ｂ点只能以ＡＣ为轴，Ｂ点到ＡＣ的垂线为半径作圆周运动，即冗余机械臂的自运动。为便于描述其运动轨迹，在轨迹圆心Ｅ处建立坐标系，其中ｚ轴由Ａ点指向Ｃ点，ｙ轴垂直于平面ＯＡＣ向外，ａ：轴可由右手法则确定，定义Ｂ点在坐标系Ｅ运动的转角为０。机械臂末端位姿常以的形式给出，其中表示末端位置，Ｗ为用欧拉角表示的末端姿态，可得末端相当于世界坐标系的旋转矩阵.   

      

         确定机械臂末端相对于世界坐标系的齐次变换矩阵,   连杆ＣＤ长度为‘则Ｃ点在Ｄ坐标，因此可求得Ｃ点在世界坐标系中的坐标,   连杆ＯＡ长度，又因为机械臂基坐标系常常和世界坐标系重合，因此点Ａ在世界坐标系中，此时可求得坐标系Ｅ每个坐标轴在世界坐标系中的表达，进一步可得坐标系Ｅ相对于世界坐标系的齐次变换矩阵,  ｔ＝ｄ２ｓｉｎ为Ｂ点到ＡＣ的距离。  已求得坐标系Ｅ到世界坐标系的齐次变换矩阵，因此可利用齐次变换求得Ｂ．１点在世界坐标系中的表达.    当给定０的值时，也就确定Ｂ点在世界坐标系的位置，由于翻肩现象的存在，此时仍可以求解出８组关节角。但在路径规划中应尽可能减少机械臂不必要的动作，为此本文在求解时不考虑翻肩现象，即一个０值可唯一求得一组满足目标末端姿态的关节角。求得机械臂逆运动学解空间。

          当处于第一象限时，求得的关节角为真实关节转角。但当Ｂ点处于第三象限时，真实关节转角，在求解过程中应依据不同情况判断。关节角和０５的求解原理相同，但要注意关节角０３的求解过程需分别在坐标系Ａ和坐标系Ｂ中进行。在不考虑翻肩现象时，第二个关节角０２的值等于可求解：当都确定后可以求得第二个关节即Ａ点坐标系相对于世界坐标系的齐次变换矩阵。同样在之前的关节都确定后可以求得该关节坐标系相对于世界坐标系的齐次变化矩阵，用于关节角求解。考虑到空间障碍物的存在，需要从己求解的关节角中选择一组最优解。从安全性考虑，希望机械臂能够尽可能地远离障碍物；从效益角度考虑，希望机械臂能长久高效的执行任务，以节约成本提高收益。

       广州登高车出租,   广州登高维修车出租,  广州登高安装车出租

           机械臂末端位姿相同情况下的两种状态。机械臂尽可能远离空间障碍物时的位姿，了远离障碍物，机械臂连杆平行于ｒ轴，虽然该位姿保证了安全性，但极不利于机械臂长期运行。受重力影响，机械臂基座将受到巨大的压力，容易造成机械臂不可逆的损坏，增加不必要的成本。右图中机械臂虽然离障碍物较近，但保持了足够的距离，认为此时机械臂安全性较高，且连杆２平行于轴，对机械臂造成磨损小，因此该姿态为理想目标姿态。当机械臂末端位姿确定时，机械臂关节１、２、４、６、７的位置也确定，因此Ｂ点的位置决定了障碍物与机械臂之间的距离。由以上逆运动学推导可知，Ｂ点位置只与单一变量０有关，因此障碍物与机械臂的距离为自变量０的函数，当机械臂与障碍物保持足够距离时即认为机械臂处于安全状态，不再受障碍物的影响，引入本文第三章排斥速度的定义并重新赋予其物理意义。在障碍物建立排斥势场对进入其影响范围的机械臂产生的排斥力定义。  排斥力也是自变量０的函数。当机械臂与障碍物距离大于Ｐ时，障碍物对机械臂产生的排斥力为０；当机械臂与障碍物距离小于Ｐ时，障碍物对机械臂产生的排斥力随距离的减小而增加；当障碍物与机械臂距离等于０时，产生的排斥力达到最大值１。因此以排斥力为目标函数优化求解，可得到机械臂与障碍物距离在合理范围的位姿。考虑到障碍物可能离机械臂较远，都不对机械臂产生影响，此时机械臂处于任何位姿都不会影响排斥力Ｐ的取值。  优化该目标函数求得的０，带入机械臂逆运动学求解，便可得唯一机械臂位姿。该位姿保证了机械臂与障碍物保持足够的距离，且保证机械臂连杆２尽可能平行于ｚ轴，对机械臂造成的损失最小。求得的这组机械臂关节角即认为最优解。关节空间采样关节空间采样对应Ａｌｇｏｒｉｔｈｎｉ２中第四行。通常ＲＲＴ和ＲＲＴ＊算法使用带目标偏向的均匀采样.      表示当生成的随机数大于１－Ｑ时，将目标点作为随机采样点用于计算，即以概率１－ａ采样目标点，以概率ａ均匀采样整个状态空间Ｘ。该采样方法简单，利于搜索树等概率向未知空间探索，同时保证主要搜索方向朝目标点进行。但该方法导致过多不必要的采样，增加了算法复杂度，针对此做了不少改进。但考虑到本文离线规划的主要目的是对机械臂整个关节空间的探索，减少在线规划的搜索时间，因此本文采用类似的采样方法。不同的是，虽然己经通过机械臂逆运动学求解出了给定环境下的最优关节目标，但在动态环境中无法保证关节目标一直处于关节空间的自由区域，当最优关节目标被障碍物区域覆盖时，需迅速寻找新的关节目标用于规划。

             无论是ＲＲＴ算法中直接寻找最近点作为新节点的父节点添加到搜索树中，还是ＲＲＴ＊算法中寻找新节点的邻近节点，然后从邻近点中选择评价函数最小的节点作为其父节点添加到搜索树中，都需要进行邻近点的查找。因此寻找邻近点算法的复杂度极大地影响规划算法复杂度。随着搜索树的不断生长，节点数增多，每次查找新节点的邻近节点就要全部遍历一遍己有节点显然效率太低。ＲＴ－ＲＲＴ＊算法对二维空间进行栅格化，每采样一个新节点，首先查询同一栅格内的节点，如果栅格内不包含任何节点，则查询相邻栅格的节点，该方法有效提高了邻近点的查找效率。但对于高自由度机械臂，其规划空间维度也较高，对高讳空间直接进行栅格化开销仍然过大。ＫＤ树是一种分割ｋ维数据空间的数据结构，常用于多维空间关键数据的搜索，例如ＳＩＦＴ算法中的特征点匹配。相比于直接对空间栅格化，ＫＤ树利用己有数据按照划分规则对空间进行划分，建立起了数据之间的索引关系，使得能高效率的查找节点的邻近节点。ＫＤ树本质上一个二叉树，其节点为ｋ维空间中的点。树中的每一个非叶子节点都可认为是生成划分空间超平面的节点，其中超平面的左半空间为该节点的左子树，右半空间为该节点的右子树。每一个节点只能用于划分空间中的一维。对于机械臂关节空间，若其中一个状态节点利用其关节角１的值沿心轴向划分，则不能再用于划分轴向０２。ＫＤ树划分空间总是按照一定的顺序进行，例如第一个节点划分轴向内，则第二个节点划分轴向馬，当第七个节点划分完轴向心后再依次循环。ＲＲＴ类算法在执行过程中不断采点，因此ＫＤ树的构建也需要动态增长。ＫＤ树增加点常采用如下方法：首先从根节点开始遍历树，按照一定规则决定进入左子树还是右子树，当遍历到树的叶子节点时，将新节点作为叶子节点的左子节点或右子节点添加到树中。这种加点的方法容易导致树不平衡，降低节点的搜索效率。ＦＬＡＮＮ［库提供了一种增量式的加点方法，当新增加的节点数过多时将重新构造ＫＤ树以维持树的平衡。本章算法主要使用ＦＬＡＮＮ库提供的方法构建ＫＤ树。ＫＤ树中查找邻近点的方法主要有两种。一种是范围查询，即给定查询点和查询距离，寻找数据集中所有与查询点距离小于给定查询距离的节点，对应Ａｌｇ〇ｒｉｔｈｍ２中第八行，用于节点的重新布线。另一种是Ｋ近邻查询，即给定查询点和需要查找的节点数Ｋ，从数据集中查找与查询点距离最近的Ｋ个节点，当欠＝１时即查询最近节点，对应Ａｌｇｏｒｉｔｈｍ２中第五行。

         广州登高车出租,   广州登高维修车出租,  广州登高安装车出租