[发明专利]一种距离限制的工业机器人运动学参数估计方法

权利要求书

1.一种工业机器人的运动学参数估计方法，是应用于工业机器人的标定过程中，并在所述工业机器人的末端装载有位置测量装置；其特征是，所述运动学参数估计方法是按如下步骤进行：

步骤1、在所述位置测量装置的测量范围内，建立工业机器人基坐标系O_B、机器人末端坐标系O_S，工具坐标系O_t，并将测量装置坐标系设置为世界坐标系O_W；

步骤2、建立所述工业机器人的运动学参数模型；

步骤2.1、对于j轴驱动装置输出的n自由度工业机器人中的每一个轴建立一个关节坐标系，通过标准D-H参数建立相邻第i个和第i+1个关节坐标系之间的位姿关系T_i；i∈[1,j-1]；

步骤2.2、由所述位姿关系T_i获得机器人末端坐标系O_S相对于机器人基坐标系O_B的位姿关系^BT_S；

步骤3、工具坐标系O_t的标定；

步骤3.1、根据工业机器人的各关节之间的位姿关系与机器人末端坐标系O_S的关系，通过单轴旋转法，得到机器人末端坐标系O_S的原点位置和机器人末端坐标系O_S的Z轴方向；

步骤3.2、以工业机器人末端执行器旋转不同角度得到的两点距离以及测量装置得到相同两点的距离误差最小为目标函数，利用最小二乘法优化所述目标函数，从而得到工具坐标系O_t与机器人末端坐标系O_S的位姿关系记为^ST_t；

步骤4、建立工业机器人的运动学参数估计距离误差模型；

步骤4.1、通过机器人末端坐标系O_S相对于机器人基坐标系O_B的位姿关系^BT_S和工具坐标系O_t相对于机器人末端坐标系O_S的位姿关系^ST_t，利用式(1)得到工具坐标系O_t相对于机器人基坐标系O_B的位姿关系^BT_t：

式(1)中，R_3×3为工具坐标系O_t相对机器人基坐标系O_B的旋转矩阵；p_x,p_y,p_z分别表示工具坐标系O_t相对机器人基坐标系O_B在X轴、Y轴和Z轴的平移向量；

步骤4.2、当仅考虑位姿关系^BT_t中的位置向量p＝(p_x,p_y,p_z)^T，利用式(2)得到式(1)的化简式：

p＝g(θ,η′)    (2)

式(2)中，θ表示工业机器人各轴旋转角度，即为输入变量，η′为待估计的运动学参数，即为待估计的常量；g表示通过θ和η′得到位置向量p的函数；

相对于机器人基坐标系O_B，工具坐标系O_t在工业机器人运动轨迹中的第n个位形的位置向量为P_n＝(p_x,n,p_y,n,p_z,n)^T＝g(θ_n,η′)；θ_n表示工业机器人第n个位形的各轴旋转角度；

相对于测量装置坐标系O_W，工具坐标系O_t在测量装置中第n个位形的位置为P_n′＝(p′_x,n,p′_y,n,p_z′_,n)^T；

利用式(3)构造第n个位形的相对距离误差函数f_n(θ_n,η′)：

f_n(θ_n,η′)＝||P_n-P_n-1|||₂-||P_n′-P_n′_-1||₂＝||g(θ_n,η′)-g(θ_n-1,η′)||₂-||P_n′-P_n′_-1||₂(3)

式(3)中，P_n-1表示相对于机器人基坐标系O_B，工具坐标系O_t在工业机器人运动轨迹中的第n-1个位形的位置向量，P_n′_-1表示相对于测量装置坐标系O_W，工具坐标系O_t在测量装置中第n-1个位形的位置，θ_n-1表示工业机器人第n-1个位形的各轴旋转角度；

步骤4.3、根据相对距离误差函数f_n(θ_n,η′)构造误差函数e(η′)＝[f₁(θ₁,η′),f₂(θ₂,η′),…,f_n(θ_n,η′),…,f_m(θ_m,η′)]^T，m表示测量数据的组数，从而利用式(4)构造最小二乘目标函数E(η′)：

步骤5、在估计运动学参数η′中，去除对最小二乘目标函数E(η′)没有影响的冗余参数，得到有效的估计参数η；

步骤6、利用Dog-Leg算法估计工业机器人的运动学参数；

定义k表示迭代次数，并令k＝0；定义最大迭代次数为kmax；

初始化第k次迭代的信赖域半径为Δ_k，将机器人理论D-H参数作为第k次迭代有效的估计参数η_k，设定三个控制误差为e₁,e₂,e₃；

步骤6.1、计算第k次迭代的第n个位形的相对距离误差函数f_n(θ_n,η_k)对应的雅可比矩阵J_n，对于m组数据，由J_n构造第k次迭代的目标函数E(η_k)的雅可比矩阵J(η_k)＝[J₁ J₂…J_n…J_m]^T，从而构造第k次迭代Gauss-Newton法的下降方向和最速下降法的下降方向

步骤6.2、如果则转到步骤6.7，否则根据η_k求解第k次迭代的误差函数e(η_k)，如果||e(η_k)||₂≤e₃或者第k次迭代的信赖域半径Δ_k≤e₂(||η_k||+e₂)，则转到步骤6.7，否则执行步骤6.3；

步骤6.3、当使用为目标函数E(η_k)的下降方向时，计算第k次迭代沿最速下降法下降方向移动的步长λ_k和比例参数β_k，从而利用式(5)计算Dog-Leg算法的下降方向

如果则转到步骤6.7，否则执行步骤6.4；

步骤6.4、令新的运动学参数计算增益比ρ，若ρ＞0，则更新第k+1次迭代有效的估计参数η_k+1＝η_new；否则，更新第k+1次迭代有效的估计参数η_k+1＝η_k；

步骤6.5、利用式(6)更新第k次迭代的信赖域半径Δ_k，从而得到第k+1次迭代的信赖域半径Δ_k+1：

式(6)中，σ表示所设定的阈值；

步骤6.6、令k+1赋值给k后，判断k＜kmax是否成立，若成立，则转到步骤6.1，否则，执行步骤6.7；

步骤6.7、输出第k次迭代有效的估计参数η_k即为所需要的D-H参数估计值。