[发明专利]一种基于视觉辅助定位的机器人自标定方法

权利要求书

1.一种基于视觉辅助定位的机器人自标定方法，其特征在于：包括以下步骤：

(1)建立机器人运动学模型

建立D-H法与MD-H法相结合的机器人运动学模型，将关节坐标系i-1到关节坐标系i的变换过程描述为A_i，A_i＝f(α_i-1,a_i-1,d_i,θ_i,β_i)，则机器人末端坐标系n相对于基坐标系的位姿矩阵⁰T_n为：

⁰T_n＝A₀·A₁·...·A_n

由D-H法，上式中a_i-1表示相邻两个关节的关节轴线J_i-1与J_i的公垂线距离，α_i-1表示由J_i-1与上述公垂线构成平面P，J_i与平面P的夹角，d_i表示一个关节前后相邻两条公垂线之间的距离，θ_i表示一个关节前后相邻两条公垂线在以关节轴线J_i为法线的平面上的投影的夹角，由MD-H法，相邻两平行轴实际无法绝对平行，J_i轴实际位置应在与之有微小转角的J'_i轴处，β_i表示实际J'_i轴与J_i-1轴之间存在的微小转角偏差；

(2)建立机器人末端位置误差模型

按照微分变换的思想对A_i进行全微分，得到由连杆几何参数误差造成的相邻关节坐标系间的微分摄动齐次矩阵dA_i：

δA_i是关节坐标系i相对于关节坐标系i-1的微分变换，则机器人相邻两连杆之间的实际齐次坐标变换即A_i+A_iδA_i，那么机器人末端坐标系相对于基坐标系的实际齐次变换矩阵T_R为：

其中，T表示机器人末端坐标系相对于基坐标系的名义齐次变换矩阵，将上式展开，并略去高阶摄动项，化简后得到下式：

其中，ΔP＝[dP_x dP_y dP_z]^T是机器人位置误差矩阵，其中各下标分别表示位置误差在x,y,z方向的分量；J为3×(4n+1)连杆参数的微分变换雅可比矩阵，其中，J的下标分别表示雅克比矩阵关于D-H各参数α,a,d,θ,β和关于x,y,z方向的分量；ΔX＝[Δα ΔaΔθ Δd Δβ]^T为(4n+1)×1连杆参数误差矩阵；

(3)建立基于平面约束的机器人连杆参数误差模型

设为约束平面上第i个接触点的名义位置值，可通过机器人正运动学直接计算，J_i为该位置处的雅克比矩阵，可通过关节角度值计算得到，实际位置P_i^R＝P_i^N+J_iΔX，则相邻两接触点间的偏差向量：

其中，ΔJ_i＝J_i-J_i-1；

同样地，那么由相邻的两个偏差向量可构建一垂直于平面的名义法向量：

式中，ΔN_i为垂直于平面的名义法向量，和分别为在x、y、z方向表达该法向量的方向向量和雅可比向量；

同样地，

式中，ΔN_i+1为垂直于平面的另一名义法向量，和分别为在x、y、z方向表达该法向量的方向向量和雅可比向量；

由于ΔN_i和ΔN_i+1都垂直于平面，那么：

式中，和分别为在x、y、z方向表达描述ΔN_i和ΔN_i+1关系的方向向量和雅可比向量；

(4)定位约束平面

对每个相机进行张正友内参标定及手眼标定，通过双目视觉获取约束平面在左右相机中的图像并提取单幅图像中的目标点信息，通过立体匹配即可定位约束平面在机器人基坐标系下的三维位置信息；

(5)驱动机器人对约束平面进行测量

将立体匹配后得到的约束平面位置值输入到机器人控制系统，驱动机器人对约束平面进行接触式测量，当测量头输出有接触信号时，立即记录当前各关节角度值，并对下一个约束点进行测量，若采集N+3个点：N₀,N₁,…,N_N+2，则有：

HΔX+S＝0

其中，N表示在初始采集3个点的基础上另外连续采集的测量点数量，由于每4个点可构成步骤(3)中的一个等式，且每个等式中在x,y,z方向各能产生一个方程，若共采集N+3个点，则可产生3N个方程，系统一共有4n+1个未知参数，因此约束点的数量要满足3N≥4n+1；

(6)机器人连杆参数辨识

通过改进的最小二乘法，对机器人运动学参数误差进行辨识，如下：

ΔX＝-(H^TH+μI)^-1H^TS

其中，μ叫做组合系数；I为单位矩阵。