CN110653805A - 笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法 - Google Patents

Abstract

笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法，涉及冗余机械臂任务约束路径规划技术，是为了实现任务约束下在笛卡尔空间，为七自由度机械臂末端执行器规划无碰撞路径，解决现有规划方法大多只适用于关节空间规划，而无法保证末端轨迹优化的问题。本发明提出一种笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法，在笛卡尔空间采用一种改进的FMT*规划算法，为机械臂规划出一条满足任务约束的末端无碰撞路径，既实现了末端轨迹的优化，也保证了机械臂运动的合理性。所述方法包括：给出了一种笛卡尔空间下基于任务函数的机械臂末端约束位姿空间的描述方法、采用高斯采样替代原有算法的采样方案、提出一种基于上述约束位姿空间的距离度量、采用一种基于臂构型描述的方式判断采样点的有效性及局部连接的有效性。

Description

笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法

技术领域

本发明涉及冗余机械臂任务约束路径规划技术。

背景技术

目前，由于可用臂构型难以完整描述和缺乏有效的逆运动学算法，基于关节空间随机采样的规划方法被广泛用于冗余机械臂在任务约束下的路径规划中。但是任务约束通常是施加于末端笛卡尔空间中的，在关节空间的随机采样规划通常是将任务约束直接映射到关节空间，在关节空间中进行随机采样，许多约束条件(例如，机械臂末端执行器上的位姿约束)定义的约束流形在关节空间中占据的体积无穷小，采样点在约束流形中的概率几乎为零，通过随机抽样发现位于这样一个流形上的构型是极不可能的。

现有的几种方法，采取从关节空间的点来寻找约束流形上的点的策略(例如，松弛、投影、切空间等)。然而，如果约束条件定义了一个具有许多折叠的复杂曲面流形，那么样本在关节空间中的均匀分布将不会转化为在约束流形中的均匀分布，这将大大降低这些方法的效率。对于低自由度机器人，我们可以通过对关节空间进行详尽的采样来计算约束流形，但是对于高自由度机器人，计算这些流形代价太大。并且由于任务通常在笛卡儿空间描述，在关节空间进行采样的方法，任务表示不直观，存在无法实现末端轨迹的优化问题。理想情况下，我们希望直接在笛卡尔空间上进行采样，而不是在关节空间中，因为任务通常在笛卡儿空间进行描述，应用上述方法时，仍需将笛卡儿参数转化到关节空间中。

发明内容

本发明是为了实现在笛卡尔空间下，为七自由度机械臂末端执行器规划无碰撞路径，解决现有规划方法只适用于关节空间规划，而无法保证末端轨迹优化的问题，从而提供一种笛卡尔空间下的七自由度冗余机械臂任务约束路径规划规划方法。

笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法，其特征是：它包括以下步骤：

步骤一、改进FMT*算法模型的构建步骤，具体包括：

步骤一一、笛卡尔空间下基于任务约束函数定义机械臂末端的约束位姿空间；

步骤一二、根据步骤一一定义的约束位姿空间进行高斯采样，以机械臂末端起始点为基础构建路径树节点集；

步骤一三、根据步骤一二采集到的采样点，采用末端约束位姿空间距离度量评价路径代价，返回两采样点之间的距离成本；

步骤一四、根据步骤一三返回两采样点之间的距离成本，返回路径树末端成本最低的采样点；

步骤一五、根据步骤一四获得的路径树成本最低的采样点选择新生长点，所述新生长点的选择原则是：以路径树末端上成本最低的采样点为圆心，预设值r_n为半径的圆内所有非路径树的采样点为生长点集；

步骤一六、根据步骤一五获得的生长点集，随机选取一个采样点作为新生长点对其进行基于臂构型描述的采样点有效性判断，如果该点判断结果是有效，将该点作为路径树有效的新生长点，并执行步骤一七；否则，执行步骤一六；

步骤一七、根据步骤一六获得的新生长点选取近邻采样点，所述近邻采样点的选择原则是：以新生长点为圆心，以预设r_n为半径的圆内路径树上的采样点为近邻采样点集；

步骤一八、根据步骤一七获得的近邻采样点集，基于距离成本最小的原则从中选取最近邻采样点，并与新生长点进行基于臂构型描述的局部连接有效性判断，如果该连接判断结果是有效，将该最近邻采样点作为新生长点的父节点；否则，执行步骤一五，选取新的生长点；

步骤一九、根据步骤一八获得的父节点与新生长点进行路径树的扩展，将该新生长点从生长点集中删除，如果生长点集不为空，则执行步骤一五；否则，执行步骤一四，循环直到目标点加入到路径树中，或者报告失败。

步骤二、对机械臂起始状态数据进行采集；

步骤三、对机械臂的任务约束进行设定；

步骤四、对机械臂的目标位姿进行设定；

步骤五、采用上位机根据步骤一获得的改进FMT*算法模型对步骤二采集的机械臂起始状态数据、步骤三设定的机械臂的任务约束和步骤四设定的机械臂目标位姿进行机械臂任务约束下的路径规划，获得满足约束的无碰撞路径。

本发明提出一种笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法，是为了实现在笛卡尔空间采用一种改进的FMT*规划算法，为机械臂规划出一条满足任务约束的末端无碰撞路径，实现末端轨迹的优化。与此同时，为实现机械臂运动的合理性，采用一种基于臂构型描述的路径点评价方法，实现冗余机械臂的避关节限位和避障碍物。

附图说明

图1是本发明的本发明的笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法的原理示意图；

图2是FMT*算法的具体伪代码截图；

图3是机械臂模型以及臂角ψ的示意图；

图4是机械臂自运动形成的扫略体以及由两个包含一个半径和五个顶点的球扫略凸体构建的碰撞检测模型示意图；

图5是机械臂自运动形成的扫略体以及由两个包含一个半径和四个顶点的球扫略凸体构建的碰撞检测模型示意图；

图6是下臂上端自运动形成的轨迹示意图；

图7是图6的轨迹原理示意图；

具体实施方式

具体实施方式一、结合图1至图7说明本实施方式，笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法，其特征是：它包括以下步骤：

步骤一、改进FMT*算法模型的构建步骤，具体包括：

步骤一、改进FMT*算法模型的构建步骤，具体包括：

步骤一一、笛卡尔空间下基于任务约束函数定义机械臂末端的约束位姿空间；

步骤一二、根据步骤一一定义的约束位姿空间进行高斯采样，以机械臂末端起始点为基础构建路径树节点集；

步骤一三、根据步骤一二采集到的采样点，采用末端约束位姿空间距离度量评价路径代价，返回两采样点之间的距离成本；

步骤一四、根据步骤一三返回两采样点之间的距离成本，返回路径树末端成本最低的采样点；

步骤一五、根据步骤一四获得的路径树成本最低的采样点选择新生长点，所述新生长点的选择原则是：以路径树末端上成本最低的采样点为圆心，预设值r_n为半径的圆内所有非路径树的采样点为生长点集；

步骤一六、根据步骤一五获得的生长点集，随机选取一个采样点作为新生长点对其进行基于臂构型描述的采样点有效性判断，如果该点判断结果是有效，将该点作为路径树有效的新生长点，并执行步骤一七；否则，执行步骤一六；

步骤一七、根据步骤一六获得的新生长点选取近邻采样点，所述近邻采样点的选择原则是：以新生长点为圆心，以预设r_n为半径的圆内路径树上的采样点为近邻采样点集；

步骤一八、根据步骤一七获得的近邻采样点集，基于距离成本最小的原则从中选取最近邻采样点，并与新生长点进行基于臂构型描述的局部连接有效性判断，如果该连接判断结果是有效，将该最近邻采样点作为新生长点的父节点；否则，执行步骤一五，选取新的生长点；

步骤一九、根据步骤一八获得的父节点与新生长点进行路径树的扩展，将该新生长点从生长点集中删除，如果生长点集不为空，则执行步骤一五；否则，执行步骤一四，循环直到目标点加入到路径树中，或者报告失败。

步骤二、对机械臂起始状态数据进行采集；

步骤三、对机械臂的任务约束进行设定；

步骤四、对机械臂的目标位姿进行设定；

步骤五、采用上位机根据步骤一获得的改进FMT*算法模型对步骤二采集的机械臂起始状态数据、步骤三设定的机械臂的任务约束和步骤四设定的机械臂目标位姿进行机械臂任务约束下的路径规划，获得满足约束的无碰撞路径。

所述步骤一一、笛卡尔空间下基于任务约束函数定义机械臂末端的约束位姿空间是为了将笛卡尔空间末端路径规划问题用基于末端位姿空间的概念来统一表示，则末端的位姿在末端位姿空间中是一个点，。位姿约束下末端路径规划问题就可以定义为在末端位姿空间下为起始位姿点与目标位姿点间寻找一条满足位姿约束条件的无碰撞路径，具体步骤包括：

将任务约束下七自由度冗余机械臂所有可能的末端位姿所构成的空间，用P来表示。n表示末端约束位姿空间的维数，对应于机械臂末端的自由度数。机械臂的末端在笛卡尔空间有6个变量，所以P空间中的任意一点x包含6个参数，其中3个表示末端位置，3个表示末端姿态，可以用向量{x₀,y₀,z₀,α,β,γ}来表示。

本文考虑机械臂末端的位姿约束，只依赖于机器人的末端位姿，而不依赖于其运动的速度或加速度等其他性质。约束减少了末端自由度数量，用m表示约束后末端自由度数量，这里m<n。末端位姿的约束本文用约束函数F来表示，当位姿点x满足约束时，F(x)＝0。约束函数定义了约束下m维末端约束位姿空间，表示为：X＝{x∈P|F(x)＝0}

所述步骤一三、采用末端约束位姿空间距离度量评价路径代价，距离度量被用来评价上文所述末端位姿空间下相邻两位姿点的相邻程度，合适的距离度量是路径规划算法执行效率的关键因素。综合考虑末端运动约束，在笛卡尔空间定义一个欧拉距离函数ρ。如上文所述，分别用T和R来表示位姿点x位移分量和旋转分量，x＝(T,R)∈Pⁿ。那么这个距离度量表示为：

其中ω_t和ω_r分别表示位移距离权重和旋转距离权重；T₁和T₂是相邻两位姿点的位移分量，|T₁-T₂|则定义为在笛卡尔空间的位移距离；R₁和R₂是相邻两位姿点的旋转分量，f(R₁,R₂)是标量函数，返回R₁，R₂之间的旋转距离值；ρ_tmax是位移分量T在约束范围内的最大距离值；ρ_rmax是旋转分量R在约束范围内的最大距离值。除以ρ_tmax，ρ_rmax两最大距离值的目的是为了实现位移和旋转的归一化，便于位移距离和旋转距离按照权重进行加权求和。

对于旋转距离函数f(R₁,R₂)，需要保证其可以反映出两采样点角度之间的变化情况，以便于后文规划器选择角度之间的最短路径，为此可以将两个旋转变量R₁、R₂之间的距离表示为：

其中：

Δ(m,n)²＝(min{|n-m|,2π-|n-m|})²

所述步骤一二、在约束位姿空间进行高斯采样，理想的采样策略是较大的开放区域不需要很多的采样点，而实际需要位于靠近障碍物的复杂区域和窄道区域的采样点，因此我们希望使用一种基于高斯函数的采样策略，其采样点被加入图中的概率依赖于附近被禁止的位姿的数量。来更好的获得环境的真实连通性。具体描述如下：

我们在位姿空间(维度n)定义高斯函数：

其中σ表示高斯范围。

定义障碍函数Obs(x)，x是属于末端位姿空间中的任意一个位姿点，如果机械臂末端处于x状态时与障碍物相交，则为1，否则为0。则高斯采样概率函数为：

f(x；σ)＝∫Obs(y)φ(x-y；σ)dy

f(x；σ)表示末端位姿空间中任意一点被采样的概率，随着采样点障碍物的增多，f(x；σ)的取值就会越大。为了避开障碍物区域的位姿点，定义：

g(x；σ)＝max(0,f(x；σ)-Obs(x))

即对于障碍物空间的位姿点，函数g(x；σ)的值为0；而其他位姿点其函数值g(x；σ)＝f(x；σ)。

所述步骤一六、对新生长点其进行基于臂构型描述的采样点有效性判断，该步骤的具体过程为：

本专利选用臂角作为冗余参数来描述本文所述冗余机械臂的自运动，所述机械臂模型以及臂角ψ的定义如图3所示：

将七自由度机械臂前三个关节等效为肩关节P_s，后三个关节等效为腕关节P_w，第四关节为肘关节P_e。由P_s、P_e、P_w臂平面。由于本文所述冗余机械臂的自运动特性，它可以使肘部绕P_s和P_w的轴线旋转，形成多个臂平面。定义θ₃＝0时的臂平面为参考平面，臂角ψ为肩、肘、腕所跨越的臂平面与基准面之间的角度。

冗余机械臂自运动的范围会受关节限位和碰撞的影响，当自运动的范围为空表明机械臂受关节限位和外形的限制无法到达该位置。本文基于此改进了FMT*算法中采样点有效性判断方法，具体步骤如下所述：

步骤A、给定机械臂末端相对于基坐标的期望位置

和姿态由基于臂角的机械臂运动学模型可得各关节和臂角之间的关系：

θ₁＝atan2{sign(sinθ₂)(-a_s22 sinψ-b_s22 cosψ-c_s22),

sign(sinθ₂)(-a_s12 sinψ-b_s12 cosψ-c_s12)}

θ₂＝±acos(-a_s32 sinψ-b_s32 cosψ-c_s32)

θ₃＝atan2{sign(sinθ₂)(a_s33 sinψ+b_s33 cosψ+c_s33),

sign(sinθ₂)(-a_s31 sinψ-b_s31 cosψ-c_s31)}

θ₅＝atan2{sign(sinθ₆)(a_w23 sinψ+b_w23 cosψ+c_w23),

sign(sinθ₆)(a_w13sinψ+b_w13cosψ+c_w13)}

θ₆＝±acos(a_w33 sinψ+b_w33 cosψ+c_w33)

θ₇＝atan2{sign(sinθ₆)(a_w32 sinψ+b_w32 cosψ+c_w32),

sign(sinθ₆)(-a_w31 sinψ-b_w31 cosψ-c_w31)}

其中a_sij,b_sij,c_sij,a_wij,b_wij,c_wij分别是矩阵A_s,B_s,C_s,A_w,B_w和C_w的(i,j)元素,0x_sw是肩关节到腕关节的向量，d_se和d_ew是常向量。根据该关系可知关节4的角度由ΔP_sP_eP_w唯一确定，只与末端位姿有关；关节2和关节6的角度则依赖于末端位姿和臂角ψ的取值；关节1、关节3、关节5和关节6的角度不光依赖于末端位姿和臂角，还取决于关节2和关节6的正负。假设各关节的运动范围为

θ_i∈[-π,π]，其中，

和

分别是关节i的下限和上限。为了在[-π,π]运动范围内确定关节角度和臂角的唯一对应关系，将关节2和关节6的运动范围分为正负两个部分，从而形成了四个臂角区域：

a)臂角区域1：

即θ₂<0,θ₆<0，在该区域机械臂的可用臂角范围为：

b)臂角区域2：

即θ₂<0,θ₆>0，在该区域机械臂的可用臂角范围为：

c)臂角区域3：

即θ₂>0,θ₆<0，在该区域机械臂的可用臂角范围为：

d)臂角区域4：即θ₂>0,θ₆>0，在该区域机械臂的可用臂角范围为：其中

是关节2或者关节6为负时关节i对应的臂角有效范围；是关节2或者关节6为正时关节i对应的臂角有效范围；ψ_j<0，(j＝2,6)为第j关节在

关节运动范围内的臂角有效范围；ψ_j>0为第j关节在

运动范围内的臂角有效范围。

从而整个机械臂在该末端位姿点下的受关节限位影响的可以臂构型空间为：

随后建立各个关节限位和臂角的关系，对于关节2和关节6，其逆解和臂角的关系可描述为如下余弦函数：

cosθ_i＝asinψ+bcosψ+c

从而建立如下不等式解得ψ_j<0和ψ_j>0

对于关节1,3,5和7，在各臂角区域内其角度和臂角一一对应，可描述为如下正切函数：

正切函数在[-π,π]范围内为周期函数，为了建立臂角和关节上下限的关系，需要根据关节运动范围分段建立不等式关系。例如

时，将分为三段建立不等式组：

a)

正切函数单调连续，分子、分母小于零，因此关节i∈(1,3,5,7)在该范围的可用臂角

可以通过如下不等式组获得:

其中+和-符号的选择取决于对应余弦关节的正负。

b)θ_i∈[-π/2,π/2],正切函数分母大于零，因此关节i在该范围的可用臂角

满足如下不等式组

c)

在该范围的可用臂角满足如下不等式组

最终关节i∈(1,3,5,7)在关节限位下的可用臂角范围

为

对于不同的关节运动范围，还能建立其他不等式组，由于篇幅限制，这里不再详细给出。

步骤B、构建关节限位约束下冗余机械臂自运动扫略体的碰撞检测模型

为了剔除碰撞对机械臂的自运动的影响引入碰撞检测技术，传统的离散碰撞碰撞检测将整个自运动范围分成若干个离散臂构型来判断是否发生碰撞，计算量大，会遗漏两检测点间的碰撞，产生“隧道效应”。因此本发明提出采用SSCH构建机械臂自运动形成的扫略体，可以有效地提高算法的实时性，解决“隧道效应”。球扫掠凸体

是由半径为r球体与凸体

的Minkowski和,定义如下：

其中是由点集

形成的凸体。

球扫略直线

构建上、下臂的碰撞检测模型，当机械臂自运动时分别形成如图4和图5所示的圆锥面扫略体：

采用多个球扫略凸体近似该圆锥面扫略体。假设近似允许的误差为ε，首先计算需要的球扫略凸体个数n＝min{1,2,3,....}，即满足下式中变量n的最小值。

如图6所示下臂上端自运动形成的轨迹可用两个球扫略体近似，每个球扫略凸体可由一个半径和四个顶点组成：

因此下臂形成的扫略体可以表示为：

其中，

是点

自运动旋转ψ_l/4的点；

是点

自运动旋转ψ_l/2的点；

同理可以获得球扫略凸体

的各顶点。

同样也可以使用三个点和一个半径描述下臂上端自运动形成的轨迹，如图7所示：

则下臂自运动形成的扫略体可以表示为：

其中，

通过这两种方法可以完整、快速、精确地近似机械臂在关节限位下自运动形成的扫略体。

步骤C、使用球扫略凸体构建机器人其他部件(头，身体，腰等)和周围静止环境的碰撞检测模型，球扫掠凸体可以使用更少的点构建出精确的碰撞检测模型，调整球扫掠体凸体的点集和半径以最小的体积包含机器人各构件三维模型表面上的所有点，有效地增加碰撞检测的实时性。

步骤D、使用基于SSCH的碰撞检测方法检测机械臂自运动扫略体的碰撞模型和机器人其它构件以及周围环境碰撞模型是否发生碰撞和可能的碰撞点，剔除关节限位下可用臂角中会引起碰撞的臂角，获得满足该末端生长点、机械臂关节限位和避碰撞的可用臂构型。

步骤E、根据可用臂构型集确定采样点的有效性；

所述步骤E包括：

步骤a、判断关节4是否超出关节限位，若超出，则采样点无效；否则，执行步骤b；步骤b、判断可用臂构型集是否为空，若为空，则采样点无效；否则，采样点有效；

所述步骤一八、对最近邻采样点与新生长点进行基于臂构型描述的局部连接有效性判断，该步骤的具体过程为：

步骤A、臂角区域1和区域2、区域3和区域4的过渡条件是在两个区域的重叠部分存在一个满足如下关系的可用臂角：

1＝a_w33 sinψ_cond2+b_w33 cosψ_cond2+c_w33

在该点处对应的两组逆运动学解中关节6角度能同时为0°，从而使关节6能连续的从负值运动到正值，不会产生关节角度的跳变。

臂角区域1和区域3，区域2和区域4的过渡涉及到关节2的正负转换，因此区域过渡条件是在两个区域的重叠部分存在一个满足如下关系的可用臂角。

1＝-a_s32 sinψ_cond1-b_s32 cosψ_cond1-c_s32

同理区域1和区域4，区域2和区域3的过渡条件是在两个区域的重叠部分存在一个同时满足上面两个过渡条件的可用臂角。

步骤B、根据过渡条件确定局部连接的有效性；

所述步骤B、根据过渡条件确定局部连接的有效性，包括：

(6)根据获得的可用的臂角范围和过渡条件确定新生长点的有效性，具体步骤如下：

步骤a、判断生长点和父节点是否在同一个臂角区域内，若在同一个臂角区域内，则局部连接有效；否则，执行步骤b；

步骤b、如果不在同一个臂角区域，则根据臂角过渡条件判断是否存在过渡点，如果存在，则局部连接有效；否则，局部连接无效。

Claims (9)

1.笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法，其特征是：它包括以下步骤：

步骤一、改进FMT*算法模型的构建步骤，具体包括：

步骤一一、笛卡尔空间下基于任务约束函数定义机械臂末端的约束位姿空间；

步骤一二、根据步骤一一定义的约束位姿空间进行高斯采样，以机械臂末端起始点为基础构建路径树节点集；

步骤一三、根据步骤一二采集到的采样点，采用末端约束位姿空间距离度量评价路径代价，返回两采样点之间的距离成本；

步骤一四、根据步骤一三返回两采样点之间的距离成本，返回路径树末端成本最低的采样点；

步骤一五、根据步骤一四获得的路径树成本最低的采样点选择新生长点，所述新生长点的选择原则是：以路径树末端上成本最低的采样点为圆心，预设值r_n为半径的圆内所有非路径树的采样点为生长点集；

步骤一六、根据步骤一五获得的生长点集，随机选取一个采样点作为新生长点对其进行基于臂构型描述的采样点有效性判断，如果该点判断结果是有效，将该点作为路径树有效的新生长点，并执行步骤一七；否则，执行步骤一六；

步骤一七、根据步骤一六获得的新生长点选取近邻采样点，所述近邻采样点的选择原则是：以新生长点为圆心，以预设r_n为半径的圆内路径树上的采样点为近邻采样点集；

步骤一八、根据步骤一七获得的近邻采样点集，基于距离成本最小的原则从中选取最近邻采样点，并与新生长点进行基于臂构型描述的局部连接有效性判断，如果该连接判断结果是有效，将该最近邻采样点作为新生长点的父节点；否则，执行步骤一五，选取新的生长点；

步骤一九、根据步骤一八获得的父节点与新生长点进行路径树的扩展，将该新生长点从生长点集中删除，如果生长点集不为空，则执行步骤一五；否则，执行步骤一四，循环直到目标点加入到路径树中，或者报告失败。

步骤二、对机械臂起始状态数据进行采集；

步骤三、对机械臂的任务约束进行设定；

步骤四、对机械臂的目标位姿进行设定；

步骤五、采用上位机根据步骤一获得的改进FMT*算法模型对步骤二采集的机械臂起始状态数据、步骤三设定的机械臂的任务约束和步骤四设定的机械臂目标位姿进行机械臂任务约束下的路径规划，获得满足约束的无碰撞路径。

2.根据权利要求1所述的笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法，其特征在于步骤一一、笛卡尔空间下基于任务约束函数定义机械臂末端约束位姿空间，包括：

将任务约束下七自由度冗余机械臂所有可能的末端位姿所构成的空间，用P来表示。n表示约束位姿空间的维数，对应于机械臂末端的自由度数。机械臂的末端在笛卡尔空间有6个变量，所以P空间中的任意一点x包含6个参数，其中3个表示末端位置，3个表示末端姿态，可以用向量{x₀,y₀,z₀,α,β,γ}来表示。

本文考虑机械臂末端的位姿约束，只依赖于机器人的末端位姿，而不依赖于其运动的速度或加速度等其他性质。约束减少了末端自由度数量，用m表示约束后末端自由度数量，这里m<n。末端位姿的约束本文用约束函数F来表示，当位姿点x满足约束时，表示为：F(x)＝0

约束函数定义了约束下m维末端约束位姿空间，表示为：

X＝{x∈P|F(x)＝0}。

3.根据权利要求1所述的笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法，其特征在于步骤一二中约束位姿空间进行高斯采样，包括：

在位姿空间(维度n)定义高斯函数：

其中：σ表示高斯范围。

定义障碍函数Obs(x)，x是属于约束位姿空间中的任意一个位姿点，如果机械臂末端处于x状态时与障碍物相交，则为1，否则为0。则高斯采样概率函数为：

f(x；σ)＝∫Obs(y)φ(x-y；σ)dy

f(x；σ)表示约束位姿空间中任意一点被采样的概率，随着采样点障碍物的增多，f(x；σ)的取值就会越大。为了避开障碍物区域的位姿点，定义：

g(x；σ)＝max(0,f(x；σ)-Obs(x))

即对于障碍物空间的位姿点，函数g(x；σ)的值为0；而其他位姿点其函数值g(x；σ)＝f(x；σ)。

4.根据权利要求1所述的笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法，其特征在于步骤二中对机械臂末端数据进行采集，采集的数据包括：机械臂末端的起始状态、关节限位和障碍物信息。

5.根据权利要求1所述的笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法，其特征在于步骤一三中采用末端约束位姿空间距离度量评价路径代价，返回两采样点之间的距离成本的原则和方法是：

在笛卡尔空间定义一个欧拉距离函数ρ；如上文所述，分别用T和R来表示位姿点x位移分量和旋转分量，x＝(T,R)∈Pⁿ；那么这个距离度量表示为：

其中ω_t和ω_r分别表示位移距离权重和旋转距离权重；T₁和T₂是相邻两位姿点的位移分量，|T₁-T₂|则定义为在笛卡尔空间的位移距离；R₁和R₂是相邻两位姿点的旋转分量，f(R₁,R₂)是标量函数，返回R₁，R₂之间的旋转距离值；ρ_tmax是位移分量T在约束范围内的最大距离值；ρ_rmax是旋转分量R在约束范围内的最大距离值。除以ρ_tmax，ρ_rmax两最大距离值的目的是为了实现位移和旋转的归一化，便于位移距离和旋转距离按照权重进行加权求和。

对于旋转距离函数f(R₁,R₂)，需要保证其可以反映出两采样点角度之间的变化情况，以便于后文规划器选择角度之间的最短路径，为此可以将两个旋转变量R₁、R₂之间的距离表示为：

其中：

Δ(m,n)²＝(min{|n-m|,2π-|n-m|})²。

6.根据权利要求1所述的笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法，其特征在于步骤一六中对新生长点其进行基于臂构型描述的采样点有效性判断的原则和方法是：

步骤A、给定机械臂末端相对于基坐标的期望位置

和姿态

由基于臂角的机械臂运动学模型可得各关节和臂角之间的关系：

θ₁＝atan2{sign(sinθ₂)(-a_s22sinψ-b_s22cosψ-c_s22),

sign(sinθ₂)(-a_s12sinψ-b_s12cosψ-c_s12)}

θ₂＝±acos(-a_s32sinψ-b_s32cosψ-c_s32)

θ₃＝atan2{sign(sinθ₂)(a_s33sinψ+b_s33cosψ+c_s33),

sign(sinθ₂)(-a_s31sinψ-b_s31cosψ-c_s31)}

θ₅＝atan2{sign(sinθ₆)(a_w23sinψ+b_w23cosψ+c_w23),

sign(sinθ₆)(a_w13sinψ+b_w13cosψ+c_w13)}

θ₆＝±acos(a_w33sinψ+b_w33cosψ+c_w33)

θ₇＝atan2{sign(sinθ₆)(a_w32sinψ+b_w32cosψ+c_w32),

sign(sinθ₆)(-a_w31sinψ-b_w31cosψ-c_w31)}

其中：a_sij,b_sij,c_sij,a_wij,b_wij,c_wij分别是矩阵A_s,B_s,C_s,A_w,B_w和C_w的(i,j)元素,⁰x_sw是肩关节到腕关节的向量，d_se和d_ew是常向量。根据该关系可知关节4的角度由ΔP_sP_eP_w唯一确定，只与末端位姿有关；关节2和关节6的角度则依赖于末端位姿和臂角ψ的取值；关节1、关节3、关节5和关节6的角度不光依赖于末端位姿和臂角，还取决于关节2和关节6的正负。假设各关节的运动范围为(i＝1,2,…,7)θ_i∈[-π,π]，其中，

和

分别是关节i的下限和上限。为了在[-π,π]运动范围内确定关节角度和臂角的唯一对应关系，将关节2和关节6的运动范围分为正负两个部分，从而形成了四个臂角区域：

臂角区域1：

即θ₂<0,θ₆<0，在该区域机械臂的可用臂角范围为：

臂角区域2：

即θ₂<0,θ₆>0，在该区域机械臂的可用臂角范围为：

臂角区域3：即θ₂>0,θ₆<0，在该区域机械臂的可用臂角范围为：

臂角区域4：

即θ₂>0,θ₆>0，在该区域机械臂的可用臂角范围为：其中

(i＝1,3,5,7)是关节2或者关节6为负时关节i对应的臂角有效范围；

(i＝1,3,5,7)是关节2或者关节6为正时关节i对应的臂角有效范围；ψ_j<0，(j＝2,6)为第j关节在

关节运动范围内的臂角有效范围；ψ_j>0为第j关节在

运动范围内的臂角有效范围。

从而整个机械臂在该末端位姿点下的受关节限位影响的可以臂构型空间为：

随后建立各个关节限位和臂角的关系，对于关节2和关节6，其逆解和臂角的关系可描述为如下余弦函数：

cosθ_i＝asinψ+bcosψ+c

从而建立如下不等式解得ψ_j<0和ψ_j>0

对于关节1,3,5和7，在各臂角区域内其角度和臂角一一对应，描述为如下正切函数：

正切函数在[-π,π]范围内为周期函数，为了建立臂角和关节上下限的关系，需要根据关节运动范围分段建立不等式关系；例如

时，将分为三段建立不等式组：

1)

正切函数单调连续，分子、分母小于零，因此关节i∈(1,3,5,7)在该范围的可用臂角

可以通过如下不等式组获得:

其中+和-符号的选择取决于对应余弦关节的正负。

2)θ_i∈[-π/2,π/2],正切函数分母大于零，因此关节i在该范围的可用臂角

满足如下不等式组

3)

在该范围的可用臂角

满足如下不等式组

最终关节i∈(1,3,5,7)在关节限位下的可用臂角范围

为：

对于不同的关节运动范围，还能建立其他不等式组，由于篇幅限制，这里不再详细给出。

步骤B、构建关节限位约束下冗余机械臂自运动扫略体的碰撞检测模型：

为了剔除碰撞对机械臂的自运动的影响引入碰撞检测技术，传统的离散碰撞碰撞检测将整个自运动范围分成若干个离散臂构型来判断是否发生碰撞，计算量大，会遗漏两检测点间的碰撞，产生“隧道效应”。因此本发明提出采用SSCH构建机械臂自运动形成的扫略体，可以有效地提高算法的实时性，解决“隧道效应”；球扫掠凸体

是由半径为r球体与凸体的Minkowski和,定义如下：

其中

是由点集

形成的凸体。

采用多个球扫略凸体近似该圆锥面扫略体。假设近似允许的误差为ε，首先计算需要的球扫略凸体个数n＝min{1,2,3,....}，即满足下式中变量n的最小值：

步骤C、使用球扫略凸体构建机器人其他部件(头，身体，腰等)和周围静止环境的碰撞检测模型，球扫掠凸体可以使用更少的点构建出精确的碰撞检测模型，调整球扫掠体凸体的点集和半径以最小的体积包含机器人各构件三维模型表面上的所有点，有效地增加碰撞检测的实时性。

步骤D、使用基于SSCH的碰撞检测方法检测机械臂自运动扫略体的碰撞模型和机器人其它构件以及周围环境碰撞模型是否发生碰撞和可能的碰撞点，剔除关节限位下可用臂角中会引起碰撞的臂角，获得满足该末端生长点、机械臂关节限位和避碰撞的可用臂构型。

步骤E、根据可用臂构型集确定采样点的有效性。

7.根据权利要求6所述的笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法，其特征在于步骤E包括：

步骤a、判断关节4是否超出关节限位，若超出，则采样点无效；否则，执行步骤b；

步骤b、判断可用臂构型集是否为空，若为空，则采样点无效；否则，采样点有效。

8.根据权利要求1所述的笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法，其特征在于步骤一八中最近邻采样点与新生长点进行基于臂构型描述的局部连接有效性判断的原则和方法是：

步骤A、臂角区域1和区域2、区域3和区域4的过渡条件是在两个区域的重叠部分存在一个满足如下关系的可用臂角：

1＝a_w33sinψ_cond2+b_w33cosψ_cond2+c_w33

在该点处对应的两组逆运动学解中关节6角度能同时为0°，从而使关节6能连续的从负值运动到正值，不会产生关节角度的跳变。

臂角区域1和区域3，区域2和区域4的过渡涉及到关节2的正负转换，因此区域过渡条件是在两个区域的重叠部分存在一个满足如下关系的可用臂角。

1＝-a_s32sinψ_cond1-b_s32cosψ_cond1-c_s32

同理区域1和区域4，区域2和区域3的过渡条件是在两个区域的重叠部分存在一个同时满足上面两个过渡条件的可用臂角。

步骤B、根据过渡条件确定局部连接的有效性。

9.根据权利要求8所述的笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法，其特征在于步骤B包括：

步骤a、判断生长点和父节点是否在同一个臂角区域内，若在同一个臂角区域内，则局部连接有效；否则，执行步骤b；

步骤b、如果不在同一个臂角区域，则根据臂角过渡条件判断是否存在过渡点，如果存在，则局部连接有效；否则，局部连接无效。

Images