《机器人建模与控制 》 实验报告 项目7（仿真实验）-机械臂力矩控制.pdf

实验报告

课程名称：机器人学I强化训练与实践

实验题目：机械臂力矩控制

指导老师：刘山

学生姓名与学号：

一、力控原理解析

1、力矩计算原理

本次力控实验采用牛顿欧拉法迭代计算力矩，其理论迭代式如下：

2、五次多项式规划原理

采用五次多项式插值方法对机械臂的末端位姿进行轨迹规划。对于五

次多项式插值方法，除了指定运动段的起点和终点的位置和速度外，还需要

指定该运动段的起点和终点的加速度。可以得到公式如下：

将自定义的插值条件代入上述三个公式，可以得到一个六元方程组，

如下所示：

六个等式的右侧的位置、速度、加速度已知，可以求解出a到a六个

05

系数的表达式如下：

将这六个系数代回到公式：

即可得到可用于速度控制的五次插值多项式。

3、力矩控制机械臂原理：

本次实验先通过五次多项式进行路径规划，即给定初末时刻的位置、速度、加速度规划

得到五次多项式，设置时间间隔为0.05s，得到每个时间点处的位置、速度、加速度，通过

牛顿欧拉法计算得到理论力矩值并发布。规划机械臂运行2s小球下落，在[0.30.10.3]处

击球。实际运行后略有误差，但能将小球准确击打至指定位置。

二、力控代码实现

1、五次多项式规划代码：

MatrixXdplanner(doubleplanning_time,MatrixXdinit_final_position,MatrixXd

init_final_velocity,MatrixXdinit_final_acceleration){

doubleinit_position,final_position,init_velocity,final_velocity,

init_acceleration,final_acceleration;

MatrixXdPoly_coef(6,6);

for(inti=0;i6;i++){

init_position=init_final_position(i,0);final_position=

init_final_position(i,1);//赋值初末位置

init_velocity=init_final_velocity(i,0);final_velocity=

init_final_velocity(i,1);//赋值初末速度

init_acceleration=init_final_acceleration(i,0);final_acceleration=

init_final_acceleration(i,1);/