一种基于动力学的机械臂柔性控制方法技术

本发明专利技术公开了一种基于动力学的机械臂柔性控制方法，该控制方法包括以下步骤：采用广义D‑H法对模型进行运动学建模；采用牛顿—欧拉公式对模型进行动力学建模；计算出关节力矩，对力矩结果采用多元函数Taylor公式进行近似估计；根据对关节速度的近似处理，进一步的简化关节力矩的表达式；给定某一段圆弧曲线，将计算结果在MATLAB中做了相应验证。本发明专利技术的一种基于动力学的机械臂柔性控制方法，本发明专利技术主要是针对高加速度下的低速运动，利用Taylor公式对速度和加速度项做了近似处理，使得计算的扭力公式变得简单，而且使用的效果与真实值相当，降低了控制器实时计算的压力。

A flexible control method of manipulator based on Dynamics

The invention discloses a flexible control method of a mechanical arm based on dynamics, the control method comprises the following steps: kinematic modeling of the model with the generalized D \u2011 h method; dynamic modeling of the model with Newton Euler formula; calculation of joint moment, approximate estimation of the moment result with the multiple function Taylor formula; approximate treatment of joint speed The expression of joint moment is further simplified. Given a certain arc curve, the calculation results are verified in MATLAB. The invention is a flexible control method of mechanical arm based on dynamics. The invention is mainly aimed at the low-speed movement under the high acceleration speed. The Taylor formula is used to approximate the speed and acceleration terms, making the calculated torque formula simple, and the effect is equivalent to the real value, reducing the real-time calculation pressure of the controller.

【技术实现步骤摘要】
一种基于动力学的机械臂柔性控制方法
本专利技术属于动力学领域，特别涉及一种基于动力学的机械臂柔性控制方法。
技术介绍
当前市场上求机械臂的关节力矩一般采用传统的牛顿—欧拉公式或者拉格朗日动力学建模，最后求出来的关节力矩公式非常复杂，对控制器的处理数据的能力要求高，而且电机反馈的速度、加速度要非常精确，这样才能有效的控制机械臂。本方案采用传统的牛顿—欧拉公式，针对五自由度机械臂，计算出关节力矩，然后采用多元函数Taylor公式对计算结果做了近似处理，解决了数据处理复杂的问题和低速下速度反馈不精确的问题。
技术实现思路
本专利技术的主要目的在于提供一种基于动力学的机械臂柔性控制方法，可以有效解决
技术介绍
中的问题。为实现上述目的，本专利技术采取的技术方案为：一种基于动力学的机械臂柔性控制方法，该控制方法包括以下步骤：(1)采用广义D-H法对模型进行运动学建模；(2)采用牛顿—欧拉公式对模型进行动力学建模；(3)计算出关节力矩，对力矩结果采用多元函数Taylor公式进行近似估计，对其中的高阶项采用梯度法进行近似处理；(4)根据对关节速度的近似处理，进一步的简化关节力矩的表达式；(5)给定某一段圆弧曲线，将计算结果在MATLAB中做了相应验证。优选的，步骤(1)中采用广义D-H法进行运动学建模，根据该五自由度机械臂的D-H参数可以得到上一个坐标系到下一个坐标系的位姿变换矩阵为Ti(θi)(i＝1,2,3.4,5)，从而姿态变换矩阵为优选的，所述步骤(2)中牛顿—欧拉公式对模型进行动力学建模包括以下步骤：(2-1)从机械臂底座开始往外递推，求出每个关节的速度、加速度、惯性力和惯性力矩在当前关节坐标系下的表示，具体如下：先求每个关节处的速度wi、加速度以及质心处的加速度再求质心处的惯性力Fi和力矩Ni：(2-2)根据末端的负载往外递推，求出各连杆上的力fi和力矩ni从而各关节的驱动力矩τi：τi＝zi·ni其中zi代表各个旋转轴旋转方向。优选的，步骤(3)中计算出关节力矩，对力矩结果采用多元函数Taylor公式进行近似估计，该多元函数Taylor公式为：机械臂在高加速度下的低速运动，关节力矩为τi(i＝1,2,3,4,5)1)对关节速度wk(k＝1,2,3,4,5)使用Taylor公式，在w＝0处展开，去掉二阶以上的项，用一阶近似替代，得到τii(w1,w2,w3,w4,w5)；2)对扭力τii(w1,w2,w3,w4,w5)，对加速度项使用Taylor公式，在加速度a＝a0处展开，去掉二阶以上的项，用一阶近似替代，得到τii(w1,w2,w3,w4,w5,a0)。优选的，步骤(4)中如果关节速度小于设置的速度时，则对扭力表达式中的速度值做近似处理，使从而，经过化简，该模型对应的最终力矩的表达式为：τ1＝-g*m5*r5*sin(θ2+θ3+θ4+θ5)*sin(θ1)+C1τ2＝-g*((l4*m5*cos(θ2-θ1+θ3+θ4))/2+(m4*r4*cos(θ2-θ1+θ3+θ4))/2-(m5*r5*cos(θ1+θ2+θ3+θ4+θ5))/2+(m5*r5*cos(θ2-θ1+θ3+θ4+θ5))/2-(l4*m5*cos(θ1+θ2+θ3+θ4))/2-(m4*r4*cos(θ1+θ2+θ3+θ4))/2)+C2τ3＝-g*((l4*m5*cos(θ2-θ1+θ3+θ4))/2+(m4*r4*cos(θ2-θ1+θ3+θ4))/2-(m5*r5*cos(θ1+θ2+θ3+θ4+θ5))/2-(l3*m4*cos(θ1+θ2+θ3))/2-(l3*m5*cos(θ1+θ2+θ3))/2-(m3*r3*cos(θ1+θ2+θ3))/2+(m5*r5*cos(θ2-θ1+θ3+θ4+θ5))/2+(l3*m4*cos(θ2-θ1+θ3))/2+(l3*m5*cos(θ2-θ1+θ3))/2+(m3*r3*cos(θ2-θ1+θ3))/2-(l4*m5*cos(θ1+θ2+θ3+θ4))/2-(m4*r4*cos(θ1+θ2+θ3+θ4))/2)+C3τ4＝-g*((l4*m5*cos(θ2-θ1+θ3+θ4))/2+(m4*r4*cos(θ2-θ1+θ3+θ4))/2-(m5*r5*cos(θ1+θ2+θ3+θ4+θ5))/2+(l2*m3*cos(θ1-θ2))/2+(l2*m4*cos(θ1-θ2))/2+(l2*m5*cos(θ1-θ2))/2+(m2*r2*cos(θ1-θ2))/2-(l3*m4*cos(θ1+θ2+θ3))/2-(l3*m5*cos(θ1+θ2+θ3))/2-(m3*r3*cos(θ1+θ2+θ3))/2+(m5*r5*cos(θ2-θ1+θ3+θ4+θ5))/2+(l3*m4*cos(θ2-θ1+θ3))/2+(l3*m5*cos(θ2-θ1+θ3))/2+(m3*r3*cos(θ2-θ1+θ3))/2-(l4*m5*cos(θ1+θ2+θ3+θ4))/2-(m4*r4*cos(θ1+θ2+θ3+θ4))/2-(l2*m3*cos(θ1+θ2))/2-(l2*m4*cos(θ1+θ2))/2-(l2*m5*cos(θ1+θ2))/2-(m2*r2*cos(θ1+θ2))/2)+C4τ5＝g*cos(θ1)*cos(θ2)*(l2*m3+l2*m4+l2*m5+m2*r2+l3*m4*cos(3)+l3*m5*cos(θ3)+m3*r3*cos(θ3)+m5*r5*cos(θ3+θ4+θ5)+l4*m5*cos(θ3+θ4)+m4*r4*cos(θ3+θ4))-sin(θ2)*(g*cos(θ1)*cos(θ3)*(l4*m5*sin(θ4)+m4*r4*sin(θ4)+m5*r5*sin(θ4+θ5))+g*cos(θ1)*sin(θ3)*(l3*m4+l3*m5+m3*r3+l4*m5*cos(θ4)+m4*r4*cos(θ4)+m5*r5*cos(θ4+θ5)))+C5其中mi为当前关节质量，li为关节长度，ri为关节质心在当前坐标系下的表示，C1、C2、C3、C4、C5为常数，g为重力加速度。优选的，步骤(5)中先利用机器人工具箱绘制某一段圆弧轨迹，再建立机器人模型。与现有技术相比，本专利技术具有如下有益效果：该一种基于动力学的机械臂柔性控制方法，主要是针对高加速度下的低速运动，利用Taylor公式对速度和加速度项做了近似处理，使得计算的扭力公式变得简单，而且使用的效果与真实值相当，降低了控制器实时计算的压力。附图说明图1为本专利技术被研究的双臂机械臂整体结构示意图；图2为本专利技术机器人工具箱绘制某一段圆弧轨迹的示意图；图3为本专利技术对应的机械臂关节力矩示意图；图4为本专利技术关节角速度示意图。具体实施方式本文档来自技高网...

【技术保护点】
1.一种基于动力学的机械臂柔性控制方法，该控制方法包括以下步骤：/n(1)采用广义D-H法对模型进行运动学建模；/n(2)采用牛顿—欧拉公式对模型进行动力学建模；/n(3)计算出关节力矩，对力矩结果采用多元函数Taylor公式进行近似估计，对其中的高阶项采用梯度法进行近似处理；/n(4)根据对关节速度的近似处理，进一步简化关节力矩的表达式；/n(5)给定某一段圆弧曲线，将计算结果在MATLAB中做了相应验证。/n

【技术特征摘要】
1.一种基于动力学的机械臂柔性控制方法，该控制方法包括以下步骤：
(1)采用广义D-H法对模型进行运动学建模；
(2)采用牛顿—欧拉公式对模型进行动力学建模；
(3)计算出关节力矩，对力矩结果采用多元函数Taylor公式进行近似估计，对其中的高阶项采用梯度法进行近似处理；
(4)根据对关节速度的近似处理，进一步简化关节力矩的表达式；
(5)给定某一段圆弧曲线，将计算结果在MATLAB中做了相应验证。


2.根据权利要求1所述的基于动力学的机械臂柔性控制方法，其特征在于，步骤(1)中采用广义D-H法进行运动学建模，根据该五自由度机械臂的D-H参数可以得到上一个坐标系到下一个坐标系的位姿变换矩阵为Ti(θi)(i＝1,2,3.4,5)，从而姿态变换矩阵为


3.根据权利要求1所述的基于动力学的机械臂柔性控制方法，其特征在于，所述步骤(2)中牛顿—欧拉公式对模型进行动力学建模包括以下步骤：
(2-1)从机械臂底座开始往外递推，求出每个关节的速度、加速度、惯性力和惯性力矩在当前关节坐标系下的表示，具体如下：
先求每个关节处的速度wi、加速度以及质心处的加速度



再求质心处的惯性力Fi和力矩Ni：



(2-2)根据末端的负载往外递推，求出各连杆上的力fi和力矩ni



从而各关节的驱动力矩τi：
τi＝zi·ni
其中zi代表各个旋转轴旋转方向。


4.根据权利要求1所述的基于动力学的机械臂柔性控制方法，其特征在于，步骤(3)中计算出关节力矩，对力矩结果采用多元函数Taylor公式进行近似估计，该多元函数Taylor公式为：



机械臂在高加速度下的低速运动，关节力矩为τi(i＝1,2,3,4,5)
1)对关节速度wk(k＝1,2,3,4,5)使用Taylor公式，在w＝0处展开，去掉二阶以上的项，用一阶近似替代，得到τii(w1,w2,w3,w4,w5)；
2)对扭力τii(w1,w2,w3,w4,w5)，对加速度项使用Taylor公式，在加速度a＝a0处展开，去掉二阶以上的项，用一阶近似替代，得到τii(w1,w2,w3,w4,w5,a0)。


5.根据权利要求1所述的基于动力学的机械臂柔性控制方法，其特征在于，步骤(4)中如果关节速度小于设置的速度时，则对扭力表达式中的速度值做近似处理，使从而，经过化简，该模型对应的最终力矩表达式为：
τ1＝-g*m5*r5*sin(θ2+θ3+θ4+θ5)*sin(θ1)+C1
τ2＝-g*((I4*m5*cos(θ2-θ1+θ3+θ4))/2+(m4*r4*cos(θ2-θ1+θ3+θ4))/2-(m5*r5*cos(θ1+θ2
+θ3+θ4+θ5))/2+(m5*r5*cos(θ2-θ1+θ3+θ4+θ5))/2-(I4*m5*cos(θ1+θ2+θ3+θ4))/2
-(m4*r4*cos(θ1+θ2+θ3+θ4))...

【专利技术属性】
技术研发人员：曾雄伟，方榆，昌浩田，
申请(专利权)人：广州弘度信息科技有限公司，
类型：发明
国别省市：广东;44