CN119407769A 基于运动学及动力学通 用建模的机械臂控制方法及系统 （山东开泰抛丸机械股份有限公司）

基于运动学及动力学通用建模的机械臂控本发明属于工业六自由度机械臂自动化仿制方法对工业六自由度机械臂进行末端执行器2结合工业六自由度机械臂各连杆、关节参数和建立的机械臂末机械臂末端执行器相对于机械臂基座坐标系结合机械臂末端的运动学模型，根据驱动器施加的力矩根据机械臂末端执行器在空间中的期望位置和姿态，结合机械通过相邻坐标系间的转换矩阵转换得到六自由度机械臂最后一个坐标系在基坐标中i轴沿关节轴Ji+1的轴向；i为Zi-1与Zi轴的交点或其公垂线与关节轴Ji的交点；i轴按照右手定则确定。从机械臂第一连杆和第一坐标原点向第六连杆和第六坐标原点外推推导坐标原点的从机械臂第六连杆和第六坐标原点向第一连杆和第一坐标原点内推推导连杆上的力、根据关节的力矩计算得出六自由度工业机械臂动力学模型中惯性矩阵、柯氏力和重从机械臂第一连杆和第一坐标原点向第六连杆和第六坐标原点外推推导坐标原点的角速3其中o,为连杆坐标系Bi原点的其中'b,为连杆坐标系Bi原点其中'ic为质心Ci的加速度，v,为连杆ig＝iRi-1i-1g，其中ig为连杆Li的重力加速度，iRig＝9.81；'F⃞"=-m,'vc,其中为连杆Li受到的惯性力，mi为连杆Li质量，'ic为质心Ci加速Li从机械臂第六连杆和第六坐标原点向第一连杆和第一坐标原点内推推导连杆上的力、力其中iFi,i-1为连杆Li上的力，iRi+1为连杆坐标系Bi+14其杆坐标系Bi-1到连杆坐标系Bi的位置矢量，iPOi,Ci为连杆坐标系Bi到质心Ci的位置矢量，由六自由度机械臂动力学模型r=M(B)⃞+C(9,6)b+G(0),且重力矢量G(θ)中含重得出柯氏力矢量c(e,6b:令可得出重力运动学模型构建模块，其用于结合工业六自由度机械臂各连杆、械臂末端的运动学模型，得到机械臂末端执行器相对于机械臂基座坐标系的齐次变换矩动力学模型构建模块，其用于结合机械臂末端的运动学模控制模块，其用于根据机械臂末端执行器在空间中的期望位置5[0002]本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技[0004]尽管Denavit_Hartenberg建模(简称D_H建模)方法及其改进方法在机械臂建模领控制方法对工业六自由度机械臂进行末端执行器轨迹[0009]结合工业六自由度机械臂各连杆、关节参数和建立的机得到机械臂末端执行器相对于机械臂基座坐6[0016]通过相邻坐标系间的转换矩阵转换得到六自由度机械臂最后一个坐标系在基坐[0023]从机械臂第一连杆和第一坐标原点向第六连杆和第六坐标原点外推推导坐标原[0024]从机械臂第六连杆和第六坐标原点向第一连杆和第一坐标原点内推推导连杆上[0030]其中为连杆坐标系Bi原点的[0034]其中'b,为连杆坐标系Bi7连杆坐标系Bi原点的线加速度，iωi为连杆坐标系Bi原点的角速度Big＝9.81；连杆坐标系Bi-1到连杆坐标系Bi的位置矢量，iPOi,Ci为连杆坐标系Bi到质心Ci的位置矢量，[0050]进一步地，由六自由度机械臂动力学模型r=M(O)6+C(e,6)6+G(6),且重力矢量G(θ)中含重力加速度g，令b=g=0,⃞=[1,1,1,1,1,1]",可得出惯性矩阵8[0061]图1是本发明实施例提供的基于运动学及动力学通用建模的机械臂控制方法流程9自由度机械臂进行末端执行器轨迹跟踪控制i与关节轴Ji的交点到公垂线ai+1与关节轴Ji的交点的有向[0083]其中，i为从机械臂基座开始向末端执行器的关节轴序号，本实施例中，i＝1,[0090]按照坐标系建立原则建立基于D-H运动学坐标系的六自由度工业机械臂坐标系，[0100]首先建立相邻坐标系间的旋转矩阵(连杆固连坐标系Bi上的坐标在Bi-1中的表[0102]基于旋转矩阵构建相邻坐标系间的转换矩阵(连杆固连坐标系Bi上的坐标在Bi-1[0105]通过相邻坐标系间的转换矩阵转换得到六自由度机械臂最后一个坐标系在基坐[0109]本实施例中，标准D-H运动学建模由Simulink实现，运动学建模函数(forward_kinematic_pack)接收输入theta(对应D-H参数中的θi变化值)，theta_offset(对应D-H参[0113]步骤201：首先从机械臂第一连杆和第一坐标原点向第六连杆和第六坐标原点外为关节轴Ji角速度，为关节轴Ji角加速度(传感器测得或由关节角速度求导获得)其中b,为连杆坐标系Bi原点的线'ic为质心Ci加速度。[0130]步骤202：其次从机械臂第六连杆和第六坐标原点向第一连杆和第一坐标原点内i的力矩；[0132]连杆Li上的力：其中iFi,i-1为连杆LMi,i-1为连杆Li上的力矩。[0137]步骤203：最后根据关节轴Ji的力矩计算得出六自由度工业机械臂动力学模型中[0138]其中，工业六自由度机[0140]由六自由度机械臂动力学模型且重力矢量G[0142]由六自由度机械臂动力学模型r=M(0)b+C(6,b)⃞+G(0),且重力矢量G(θ)中含重力加速度g可知，令i=g=0,可得出柯i=i=0,可得出重力矢量G(θ)。)1T2[0156]步骤305：重复以上步骤可以获得机械臂执行器末端轨迹对应关节2—6的旋转角于获得的动力学模型结合传统BackStepping控制方法，对比无需动力学模型的PD控制方的动力学模型结合传统BackStepping控制方法，蓝色虚线为无需动力学模型的PD控制方