[发明专利]一种六自由度机械臂空间轨迹优化方法

权利要求书

1.一种六自由度机械臂空间轨迹优化方法，其特征在于，其包括以下步骤：

步骤S1、初始化所述机械臂空间轨迹，该空间轨迹为包含n个状态点的待优化的分段线性路径γ＝(state₀，state₁，...，state_i，...，state_n-1)；

步骤S2、对该分段线性路径进行路径简化，所述路径简化包含：沿所述分段线性路径取两个随机采样点R₀、R₁，根据所述两个随机采样点R₀、R₁的位置构造出两个状态点P₀、P₁，调用碰撞检测算法，若状态点P₀、P₁的直接路径与障碍有碰撞，则迭代次数加1，否则判断状态点P₀、P₁是否在同一段线性路径上，若是，则迭代次数加1，否则，计算该两个状态点P₀、P₁的直接距离d₁，以及这两个状态点沿所述分段线性路径γ的距离d₂，若d₁＜d₂，则用线段P₀-P₁替代原有路径，消除原有路径上的状态点，迭代次数加1；

步骤S3、判断迭代次数，若迭代次数未达到最大迭代次数，则重复执行步骤S2，否则该路径简化结束，获得简化路径γ’＝(state’₀，state’₁，...，state’_i，...，state’_n-l)；

步骤S4、对获得的简化路径γ’进行路径平滑，所述路径平滑包含：在路径γ’上的每一分段路径上增加一个新的状态点，对下标索引i是除0外的偶数的状态点依次更新：在状态点state’_i-1与状态点state’_i分段的中点处取一点记为temp₁；在状态点state’_i与状态点state’_i+1分段的中点处取一点记为temp₂；在状态点temp₁与状态点temp₂分段的中点处取一点记为temp₃，分别连接状态点state’_i-1与状态点temp₃、状态点temp₃与状态点state’_i+1构成两个分段路径，调用碰撞检测算法验证所述两个分段路径的有效性，若有一条无效则此次更新结束；若均有效，计算状态点state’_i与状态点temp₃的直接距离d，若d大于某一设定阈值minChange，用状态点temp₃更新状态点state’_i，否则结束此次更新；

步骤S5、当路径γ’上的所有状态点更新完毕，迭代次数加1，若迭代次数小于设定迭代次数，重复执行步骤S4，反之路径平滑步骤结束。

2.根据权利要求1的所述优化方法，其特征在于，所述步骤S1中state_i表示为：路径γ上的状态点，表示六自由度机械臂在某一时刻在构型空间的信息，用一个6维向量表示state_i＝(r₁，r₂，…，r_j，…，r₆)，r_j代表机械臂各轴的角度，j＝1-6。