一种多自由度机械臂柔性控制方法和系统.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN113601509A(43)申请公布日2021.11.05(21)申请号202110939367.X(22)申请日2021.08.16(71)申请人安徽元古纪智能科技有限公司地址236000安徽省合肥市经济技术开发区宿松路3963号C3-311(72)发明人李小龙夏科睿彭超(74)专利代理机构合肥市浩智运专利代理事务所(普通合伙)34124代理人郑浩(51)Int.Cl.B25J9/16(2006.01)权利要求书3页说明书8页附图2页(54)发明名称一种多自由度机械臂柔性控制方法和系统(57)摘要一种多自由度机械臂柔性控制方法和系统，属于多自由度机械臂控制技术领域，解决如何在机械臂末端工具改变时，仅通过负载参数辨识的方式得到负载重力和重心位置，自动进行负载参数补偿的问题；根据机械臂特性进行运动学建模，辨识机械臂末端工具负载参数，包括重力和重心位置，获取六维力传感器力反馈，进行工具负载参数补偿；补偿后力数据代入柔性控制策略模块，计算的位姿偏移量并调整机械臂实时插补目标轨迹点，调整后的目标轨迹点逆解后发送到机械臂关节，采集机械臂运动过程中的关节位置和速度，自动进行负载参数补偿控制；方案在末端工具改变时，只需通过负载参数辨识的方式得到工具重力和重心位置，自动进行负载参数补偿，提高工作效率。CN113601509ACN113601509A权利要求书1/3页1.一种多自由度机械臂柔性控制方法，其特征在于，包括以下步骤：S1、根据机械臂连杆参数，采用MDH方式建立各关节坐标系，获得相邻两关节坐标系的齐次变换矩阵,进而计算获得机械臂运动学模型；再根据建立的机械臂运动学模型进行关节角度和末端位置姿态值之间相互求解；S2、辨识安装在机械臂的六维力传感器的负载末端参数，通过建立负载重力分量与重力矩分量及重心坐标关系方程，读取若干组末端不同姿态下的力传感器数据，采用最小二乘法辨识负载参数；S3、实时获取力传感器数据，并补偿负载末端参数影响，获得负载末端实际所受外力，并建立末端任意位置姿态下，末端实际所受外力与负载参数及传感器数据的关系式；S4、将补偿后的力数据，代入柔性控制策略模块，计算的位姿偏移量并调整机械臂实时插补目标轨迹点，将调整后的目标轨迹点逆解后发送到机械臂关节；S5、采集机械臂运动过程中的关节位置和速度，若位置和速度超过阈值则发送停止机械臂运动指令；若发生结构碰撞或接近奇异点，则发送停止机械臂运动指令。2.根据权利要求1所述的一种多自由度机械臂柔性控制方法，其特征在于，步骤S2中所述的负载参数包含传感器末端工具的负载重力和重心位置。3.根据权利要求2所述的一种多自由度机械臂柔性控制方法，其特征在于，步骤S2中所述的负载重力分量与重力矩分量及重心坐标关系方程为：负载重力计算式为：TT式中，[FxSFySFzS]为力信号分量，[MxSMySMzS]为力矩信号分量，[p]为向量p对应的反对称矩阵，(pxSpySpzS)为负载重心位置在传感器坐标系下的表示。4.根据权利要求3所述的一种多自由度机械臂柔性控制方法，其特征在于，步骤S3中所述的末端实际所受外力与负载参数及传感器数据的关系式为：式中，表示负载末端坐标系OE相对传感器坐标系Os的齐次变换矩阵；为负载重力向量转换到传感器坐标系下的力和力矩向量表示,为负载重力坐标系下的重力向量表示。5.根据权利要求4所述的一种多自由度机械臂柔性控制方法，其特征在于，步骤S4中所述的柔性控制策略采用基于位置的导纳控制模型，其控制方程为：2CN113601509A权利要求书2/3页每个控制周期内的计算公式如下：其中，M为惯性系数矩阵，B为阻尼系数矩阵，K为刚度系数矩阵，xd为期望轨迹，Fd为末端期望力和力矩，Fs为六维力读取传感器数据，Fe为将六维力传感器采集的数据经过补偿算法后得到的末端受到的力和力矩；Δx为经过导纳模型计算后的末端位姿变化量,qu为逆解计算得到的关节位置，调整M、K、B，可以使机械臂末端在不同方向上具备不同程度的柔性。6.一种多自由度机械臂柔性控制系统，其特征在于，包括：机械臂运动学模块、运动指令模块、轨迹规划模块、负载参数辨识模块、力信号采集和补偿模块；所述的机械臂运动学模块：根据机械臂连杆参数，采用MDH方式建立各关节坐标系，获得相邻两关节坐标系的齐次变换矩阵,进而计算获得机械臂运动学模型；再根据建立的机械臂运动学模型进行关节角度和末端位置姿态值之间相互求解；所述的负载参数辨识模块：辨识安装在机械臂的六维力传感器的负载末端参数，通过建立负载重力分量与重力矩分量及重心坐标关系方程，读取若干组末端不同姿态下的力传感器数据，采用最小二乘法辨识负载参数；所述的力信号采集和补偿模块：实时获取力传感器数据，并补偿负载末