[发明专利]一种基于关节角代偿的机器人末端多目标位姿逼近方法

权利要求书

1.一种基于关节角代偿的机器人末端多目标位姿逼近方法，其特征在于，所述方法包括：

当根据机器人几何参数的名义值确定各组关节角的目标值，并且控制机器人各关节运动到各组关节角的目标值之后，依据采集到的机器人末端位置和姿态数据测量值，对机器人几何参数误差进行辨识，将辨识出的机器人几何参数误差转换成机器人各关节角的校正值，结合转换获得的关节角的校正值计算得到用以控制机器人执行多目标位姿逼近动作的关节角的代偿值；

所述方法还包括以下步骤：

S1，建立基于基坐标系oxyz和关节坐标系o_ix_iy_iz_i下的机器人DH几何参数模型，所述DH几何参数模型的几何参数包括连杆偏距、连杆长度、连杆扭角、零位状态关节角，从机器人手册寻找并获取连杆偏距、连杆长度、连杆扭角、零位状态关节角的名义值d_i,a_i,α_i,θ_i，其中，i＝1,2,…,M，M为机器人关节数目且M为正整数；

S2，由机器人末端目标点位置坐标的目标值p_oj在基坐标系oxyz下沿x轴、y轴、z轴的坐标分量p_oxj、p_oyj、p_ozj和姿态坐标的目标值r_oj在基坐标系oxyz下绕x轴、y轴、z轴的偏航角μ_oj、俯仰角υ_oj、侧滚角ω_oj求解机器人逆运动学方程，确定各组关节角的目标值Q_ij，j依次取值1，2，…，N，N为目标点数，N取值大于等于30；

S3，在示教器上设定机器人各组关节角的目标值Q_ij，控制机器人各关节运动到各组关节角的目标值Q_ij；

S4，采用激光跟踪仪对安装在机器人末端G的靶标进行检测，获取机器人末端位置和姿态数据，其中机器人末端目标点j位置坐标的测量值p_mj在基坐标系oxyz下沿x轴、y轴、z轴的坐标分量分别为p_mxj、p_myj、p_mzj，姿态坐标的测量值r_mj在基坐标系oxyz下绕x轴偏航角、绕y轴俯仰角、绕z侧滚角分别为μ_mj、υ_mj、ω_mj；

S5，采用Levenberg-Marquardt算法对机器人DH几何参数模型的参数误差辨识获得连杆偏距的误差δd_i、连杆长度的误差δa_i、连杆扭角的误差δα_i、零位状态关节角的误差δθ_i；

S6，构造机器人末端目标点j的代偿位置平移矩阵P_cj和代偿姿态旋转矩阵R_cj，建立机器人末端多目标位姿逼近的目标函数g；

S7，使用乌鸦算法求解步骤S6所述的目标函数g，获取关节角的校正值q_i；

S8，计算机器人末端目标点j的各组关节角的代偿值

S9，在示教器上设定机器人各组关节角的代偿值控制机器人各关节运动到各组关节角的代偿值实现机器人末端多目标位姿逼近；

步骤S5中，所述采用Levenberg-Marquardt算法对机器人DH几何参数模型的参数误差辨识获得连杆偏距、连杆长度、连杆扭角、零位状态关节角的误差δd_i，δa_i，δα_i，δθ_i的过程包括以下步骤：

S51，计算机器人DH模型的雅克比矩阵H：

S511，构建机器人末端目标点j对应的关节i到关节i+1的名义齐次变换矩阵A_ij：

式中，d_i，a_i，α_i，θ_i分别表示关节i的连杆偏距、连杆长度、连杆扭角、零位状态关节角的名义值，Q_ij表示机器人末端目标点j对应的关节i的目标关节角；

S512，构建机器人末端目标点j在基坐标系oxyz下的名义位姿矩阵TA_j：

式中，表示A_1j，A_2j….，A_Mj连乘，TA_j为4行4列矩阵，表示为该矩阵第1、2、3行中的元素是几何参数d_i，a_i，α_i，θ_i的函数；

S513，将TA_j中前三行每一元素按照先列后行顺序依次分别对d_i，a_i，α_i,θ_i进行偏微分并忽略二阶及以上高阶项，获得机器人末端目标点j的雅克比矩阵H_j，H_j为12行4×M列矩阵，i＝1,2,…,M,M为机器人关节数目，j＝1,2,…,N，N为目标点数；

S514，由N个目标点的雅克比矩阵H_j获得机器人DH几何参数模型的雅克比矩阵H，H为12×N行4×M列矩阵；

S52：计算机器人末端N个目标点的位姿误差矩阵ΔE：

S521，由机器人末端目标点j位置坐标的目标值p_oj在基坐标系oxyz下沿x轴、y轴、z轴的坐标分量p_oxj、p_oyj、p_ozj和姿态坐标的目标值r_oj在基坐标系oxyz下绕x轴、y轴、z轴的偏航角μ_oj、俯仰角υ_oj、侧滚角ω_oj计算目标位置平移矩阵P_oj和目标姿态旋转矩阵R_oj：

P_oj＝[p_oxj p_oyj p_ozj]^T

R_oj＝R_oωjR_oυjR_oμj

其中，

S522，由机器人末端目标点j位置坐标的测量值p_mj在基坐标系oxyz下沿x轴、y轴、z轴的坐标分量p_mxj、p_myj、p_mzj和姿态坐标的测量值r_mj在基坐标系oxyz下绕x轴、y轴、z轴的偏航角μ_mj、俯仰角υ_mj、侧滚角ω_mj计算测量位置平移矩阵P_mj和测量姿态旋转矩阵R_mj：

P_mj＝[p_mxj p_myj p_mzj]^T

R_mj＝R_mωjR_mυjR_mμj

其中，

S523，由机器人末端目标点j的目标位置平移矩阵P_oj、目标姿态旋转矩阵R_oj和测量位置平移矩阵P_mj、测量姿态旋转矩阵R_mj构造机器人末端目标点j的位姿误差向量ΔE_j：

ΔE_j＝[ΔR_j(1,1) ΔR_j(2，1) ΔR_j(3，1) ΔR_j(1,2)…ΔR_j(2,3) ΔR_j(3,3) ΔP_j(1)ΔP_j(2) ΔP_j(3)]^T

其中，ΔR_j＝R_mj-R_oj，ΔP_j＝P_mj-P_oj；式中，ΔR_j为3行3列矩阵，ΔP_j为3行1列矩阵，ΔR_j(s,ν)表示取矩阵ΔR_j第s行第v列元素，s、v取值分别为1，2，3，ΔP_j(1)，ΔP_j(2),ΔP_j(3)分别表示取向量ΔP_j的第1，2，3个元素，ΔE_j为12行1列矩阵，T为对矩阵求转置；

S524，计算机器人末端N个目标点的位姿误差向量ΔE：

其中，ΔE为12×N行1列矩阵；

S53，采用Levenberg-Marquardt算法对机器人DH几何参数模型的几何参数误差δd_i,δa_i,δα_i,δθ_i进行辨识：

Δη＝[(H^T·H+μI)^-1H^T]ΔE

其中，几何参数误差向量Δη＝[δd₁,δa₁,δα₁,δθ₁,…，δd_M，δa_M，μα_M，δθ_M]^T，阻尼参数μ取值0.01，I为4×M行4×M列单位矩阵，T为对矩阵求转置。