[发明专利]一种基于BAS算法的六自由度偏置机器人逆解方法

权利要求书

1.一种基于BAS算法的六自由度偏置机器人逆解方法，其特征在于，包括以下步骤：

S1，构建手腕偏置型自动操作机器的sdh模型：手腕偏置型自动操作机器由基座、末端执行器和5个连杆以及6个旋转关节组成；基于标准的DH参数法建立各关节坐标系，坐标系0为基坐标系，坐标系i为建立在连杆i的末端即旋转关节i+1上的坐标系，坐标系i具有X_i、Y_i、Z_i轴，关节转角θ_i表示X_i轴与X_i-1轴的夹角，i＝1,2,3,4,5,6，坐标系6为末端执行器坐标系；

S2，将手腕偏置型自动操作机器的末端坐标系的目标位姿当作手腕无偏置型自动操作机器的末端坐标系的位姿，求得手腕无偏置型自动操作机器的运动学逆解的解析解即旋转关节的无偏置关节转角θ₁′至θ₆′，作为迭代初始点X₀；

S3，构建适应度函数F(X)：

；

其中X为旋转关节的关节转角θ₁至θ₆，表示引入偏置参数的坐标系6相对于坐标系0的齐次坐标变换矩阵的第p行第q列元素，p=1,2,3，q=1,2,3,4，nx、ny、nz分别为坐标系6的X₆轴与坐标系0的X₀、Y₀、Z₀轴的夹角余弦值；ox、oy、oz分别为坐标系6的Y₆轴与坐标系0的X₀、Y₀、Z₀轴的夹角余弦值；ax、ay、az分别为坐标系6的Z₆轴与坐标系0的X₀、Y₀、Z₀轴的夹角余弦值；px、py、pz为坐标系6的原点在坐标系0中的笛卡尔坐标；

S4，基于BAS算法求取手腕偏置型自动操作机器的逆运动学解，具体包括以下步骤：

S41，取初始点X₀，置迭代次数k=0；

S42，根据F(X)的2-范数||F(X)||₂是否到达设定阈值或是否达到最大迭代次数，判断是否跳出循环，是则跳出循环，否则进入下一次迭代；

S43，确定第k+1中拟更新的X_k+1，并判断该第k+1次迭代是否有效，具体包括以下步骤：

S431，计算左须位置xl_k和右须位置xr_k：xl_k=X_k+d_k*dir_k/2，xr_k=X_k-d_k*dir_k/2，

其中，X_k表示第k次迭代时旋转关节的各关节转角，也是天牛个体质心的位置，d_k表示天牛个体大小，dir_k表示天牛飞行方向归一化的向量，dir_k=dir_k′/norm(dir_k′)，dir_k′表示天牛飞行方向的随机向量，其为rands(6,1)，用于产生6*1阶随机向量的函数，norm表示求向量范数的函数；d_k通过调整函数进行控制，其满足d_k=d_k-1*[(n+1-k)/(n+1)]^ω ，其中n为迭代总次数，ω为常数，表示天牛个体大小递减的程度；

S432，计算第k+1次迭代中拟更新的X_k+1的值及相应的F(X_k+1)：

X_k+1= X_k-step_k* dir_k *sign(F_left-F_right)

其中，左须函数值F_left=||F(xl_k)||，右须函数值F_right=||F(xr_k)||，即分别对F(xl_k)和F(xr_k)取范数；sign为符号函数；step_k表示步长，每步迭代中步长以负指数次幂衰减，即：

step_k=step_k-1*e^-tk，其中t为第一衰减率；

S433，计算允许概率因子ρ，判断该第k+1次迭代是否有效：

，

其中||F(X_k)||₂、||F(X_k+1)||₂表示分别对F(X_k)、F(X_k+1)取2-范数；

当||F(X_k+1)||₂≤||F(X_k)||₂，此时ρ为1，第k+1次迭代完全有效，X_k更新为X_k+1，并转入步骤S42；当||F(X_k+1)||₂||F(X_k)||₂时，比较ρ与用于产生(0,1)之间随机数的随机函数rand()的大小，若ρ大于rand()，则第k+1次迭代有效，X_k更新为X_k+1，并转入步骤S42，否则前述第k+1次迭代无效，X_k不更新，重新进行第k+1次迭代，即转入步骤S431；

其中exp表示自然指数，即以e为底数的指数函数；M为运动系数，其满足M=2^-uk，其中u为第二衰减率。