实现空间机械臂避障的路径规划方法

摘要

本方法主要用于空间机械臂避障路径规划。包括以下步骤：建立窗口型(正方形)障碍，利用人工势场方法找出一条机械臂末端器的避障路径；通过求机械臂各连杆与窗口障碍各边的距离的碰撞检测来调整整个机械臂末端器轨迹，使得各个连杆均不与障碍物碰撞，从而得到一条能使整个机械臂(包括末端器和连杆)安全避障的末端器路径；运用基于广义雅克比矩阵的空间机械臂运动学逆解对上述的末端器路径上的位置和姿态进行连续跟踪。应用该障碍回避方法可使其针对整个空间机械臂(包括末端器和连杆)进行避障路径规划。

主权项

实现空间机械臂避障的路径规划方法，其特征在于，包括步骤：(1)建立中心点带有引力场的窗口型(正方形)障碍，计算其几何中心点，在该点上设置一个带吸引力的虚拟点，该点吸引力与目标点吸引力公式均为：$▿U\left(q\right)=\left[\begin{array}{c}\frac{\partial U}{\partial x}\\ \frac{\partial U}{\partial y}\\ \frac{\partial U}{\partial z}\end{array}\right]$(2)采用人工势场方法，建立斥力场函数和引力场函数，斥力公式为：$U_{\mathrm{att}}\left(q\right)=\frac{1}{2}{\mathrm{\xi \rho }}^2\left(q\right)$(3)判断机械臂末端器是否穿过障碍物边框内侧斥力场，若不穿过，转到下一步，若穿过，转到(5)；(4)调整中心点吸引力，使末端器从窗口内侧穿过；(5)在中心点引力和边框斥力的作用下，末端器趋于向窗口中心运动，当末端器穿过窗口向目标点运动时，中心点的引力消失，末端器受到的引力只来自目标点；(6)得到末端器的避障路径后，用逆广义雅克比矩阵跟踪该路径，结合碰撞检测公式$D_{i,j}=\frac{|(p_{i-1,i}\times A_{j-1,j})·(p_{i-1}-A_{j-1})|}{\left|\right|p_{i-1,i}\times A_{j-1,j}\left|\right|}$其中：i＝2,3,4,5,6；j＝2,3,4。D_i,j为各连杆与障碍物之间的距离，A_j为连杆前端到连杆质心的方向矢量；检测连杆与边框碰撞的深度与方向，根据返回结果，相应调整末端器轨迹，反复对各杆检测，最终得到一条使整个机械臂避障的末端器轨迹。