机械臂阻抗控制

原理分析

核心公式

阻抗控制的核心思想是通过模拟弹簧-阻尼系统的行为来控制机器人的末端运动。具体来说，阻抗控制通过调整末端的位置误差和速度误差来生成关节力矩，使得机器人的末端在受到外力时能够表现出期望的柔顺性。

阻抗控制的基本公式可以表示为：

$F=M\ddot x+B\dot x+Kx$

其中：

F 是末端受到的力（或力矩）。
M 是质量矩阵（惯性项）。
B 是阻尼矩阵（阻尼项）。
K 是刚度矩阵（弹性项）。
x 是末端的位置误差（即期望位置与实际位置之差）。
x˙ 是末端的速度误差（即期望速度与实际速度之差）。
x¨ 是末端的加速度误差（即期望加速度与实际加速度之差）。

在实际应用中，通常忽略加速度项，简化为：

$F=B\dot x+Kx$

实现步骤

计算当前末端位置和速度：
- 使用正运动学 fk 函数根据当前关节位置 current_joint_pos 计算当前末端位置 X。
- 使用雅可比矩阵 get_jacb 将当前关节速度 current_joint_vel 映射到末端速度 dX。
计算末端误差：
- 位置误差 e：期望位置 desir_end_pos 与实际位置 X 之差。
- 速度误差 de：期望速度 desir_end_vel 与实际速度 dX 之差。
生成末端力：
- 根据阻抗控制公式生成末端力 F：
$F_x=2c\cdot de_x+9k\cdot e_x$

$F_y=2c\cdot de_y+9k\cdot e_y$

$F_z=2c\cdot de_z+9k\cdot e_z$
- 这里的 2c 和 9k 是阻尼和刚度系数，可以根据实际需求调整。
映射到关节力矩：
- 使用雅可比矩阵的转置将末端力 F 映射到关节力矩 joint_tor：
$\tau =J^TF$
- 其中 J 是雅可比矩阵，F 是末端力，τ 是关节力矩。
更新误差状态：
- 根据位置误差的大小更新误差状态 e_state，用于判断是否达到控制目标。

二轴机械臂阻抗控制

机械参数建立

DH参数建立

$i$	$\alpha_{i-1}<br>$	$a_{i-1}$	$d_i$	$\theta_{i}$
1	0	$l_1$ 0.1m	0	$\theta$
2	0	$l_2$ 0.2m	0	$\theta$

旋转矩阵

质量和质心

m1=0.633kg rc1=(0.0916m,0,0)

m2=1.132kg rc2=(0.188m,0,0)

惯性张量

建立局部坐标系

测量惯性张量,注意这里的单位为 $kg/mm^2$

I1=

Ixx = 171.343 Ixy = -8.180 Ixz = 656.769

Iyx = -8.180 Iyy = 4709.994 Iyz = -1.438

Izx = 656.769 Izy = -1.438 Izz = 4628.414

I2=

Ixx = 1770.775 Ixy = 0.000 Ixz = 6438.672

Iyx = 0.000 Iyy = 44655.166 Iyz = 0.000

Izx = 6438.672 Izy = 0.000 Izz = 43215.652

正逆运动学

皮诺曹动力学库

这里我使用皮诺曹动力学库来写，也可以自己写正逆运动学算法，但是为了方便后续移植到七轴，所以我使用动力学库

以下是官网的安装教程

https://stack-of-tasks.github.io/pinocchio/download.html

ros里面用还要安装

1sudo apt-get install robotpkg-py3*-pinocchio

注意引入之后需要链接库

1cmake_minimum_required(VERSION 3.0.2)
2project(your_project_name)
3
4# 设置 C++ 标准
5set(CMAKE_CXX_STANDARD 11)
6set(CMAKE_CXX_STANDARD_REQUIRED ON)
7
8# 查找 catkin 依赖
9find_package(catkin REQUIRED COMPONENTS
10  roscpp
11  # 其他依赖项
12)
13
14# 查找 pinocchio 库
15find_package(pinocchio REQUIRED)
16
17# catkin 包配置
18catkin_package(
19  CATKIN_DEPENDS roscpp
20  # 其他依赖项
21)
22
23# 包含头文件目录
24include_directories(
25  ${catkin_INCLUDE_DIRS}
26  ${pinocchio_INCLUDE_DIRS}
27)
28
29# 添加可执行文件
30add_executable(myARM src/myARM.cpp)
31
32# 链接库
33target_link_libraries(myARM
34  ${catkin_LIBRARIES}
35  ${pinocchio_LIBRARIES}
36)

在使用urdf导入皮诺曹动力库的时候，如果要获取连杆末端的位置，需要加多一个link和joint，作为末端

1<link name="link2_end">
2        <visual>
3            <origin xyz="0 0 0" rpy="0 0 0"/>
4            <geometry>
5                <sphere radius="0.01"/> *<!-- 这里可以根据需要调整可视化的形状和大小 -->*
6            </geometry>
7            <material name="red">
8                <color rgba="1 0 0 1"/>
9            </material>
10        </visual>
11    </link>
12
13    <joint name="link2_to_link2_end" type="fixed">
14        <parent link="link2"/>
15        <child link="link2_end"/>
16        <origin xyz="0.2 0 0" rpy="0 0 0"/> *<!-- 根据实际情况设置 xyz 偏移量 -->*
17    </joint>

Eigen库

直接通过apt安装：sudo apt-get install libeigen3-dev

安装完成后，编译器会去 /usr/local/include 或者 /usr/include 目录找头文件，但找到的是eigen3，并没有Eigen和unsupported，因此需要建立一个软连接，链接到这两个文件夹即可

1#首先要确定你的Eigen3安装在/usr/local/include还是/usr/include文件夹
2cd /usr/include
3sudo ln -sf eigen3/Eigen Eigen
4sudo ln -sf eigen3/unsupported unsupported

效果

这个是做了重力补偿的效果

[IMG_0739.MOV]

这个是对机械臂做阻抗控制的效果，可以看到在末端具有一定的弹性使其保持在某一位置

[IMG_0740.MOV]

[IMG_0741.MOV]

七轴机械臂阻抗控制

机械参数建立

在关节上建立坐标轴，以便于DH参数的建立

[urdf_7dof.zip]

导出urdf文件

质量和质心

七轴的机械臂在这里使用匹诺曹库计算他的重力补偿（因为他的DH参数实在难建），所以不需要旋转矩阵和DH参数了。

基座

link1

link2

link3

link4

link5

link6

link7

以上的所有参数，需要在.urdf上进行修改。如下图位置所示

确定各个旋转轴的方向，

这里的方向，通过旋转电机同时观察角度获取，如果Z轴的方向（右手螺旋）与旋转的方向一致则为1，否则为负数（前提是在urdf中的旋转轴中z的位置为1）。

确定一下关节限位。在urdf中进行修改。

惯性张量

需要在上面进行修改和图中的一致

重力补偿调试

代码编写

代码层面，先做了一层开始的角度保护，安全检查，只有都在零点开始才可以启动机械臂

1// 安全检查
2      // 获取电机当前状态
3      // 初始化电机（7轴）
4      uint8_t safe_check_flag = 1;
5      if (safe_check_flag)
6      {
7            // 电机返回值（7个关节）
8            double joint[7] = {0};
9            for (size_t i = 0; i < 30; i++)
10            {
11                  for (int j = 0; j < 7; j++)
12                  {
13                        rb.Motors[j]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
14                  }
15                  rb.motor_send();
16                  r.sleep();
17            }
18            // 获取所有关节状态
19            for (int j = 0; j < 7; j++)
20            {
21                  joint_motor[j] = *rb.Motors[j]->get_current_motor_state();
22                  joint[j] = joint_direction[j] * joint_motor[j].position;
23            }
24
25            // 检查所有关节的绝对值是否都小于0.2
26            bool all_joints_safe = true;
27            for (int j = 0; j < 7; j++)
28            {
29                  if (fabs(joint[j]) >= 0.2)
30                  {
31                        all_joints_safe = false;
32                        break;
33                  }
34            }
35
36            // 如果有关节不满足条件，跳出
37            if (!all_joints_safe)
38            {
39                  ROS_ERROR("Motor position init error!");
40                  // // 打印所有关节位置
41                  ROS_INFO("Positions: %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
42                           joint[0], joint[1], joint[2], joint[3], joint[4], joint[5], joint[6]);
43                  // 获取电机当前状态
44                  while (ros::ok())
45                        ;
46            }
47      }

然后编写重力补偿的代码，这里调用皮诺曹动力学库

1/* 重力补偿和拖动示教 */
2      bool g = true;
3      uint32_t g_time = 10000; // 30s
4      if (g)
5      {
6            cout << "正在进行7轴重力补偿测试\n"
7                 << endl;
8
9            // 力矩（使用VectorXf）
10            VectorXf tor(7);
11
12            // 电机返回值（7个关节）
13            double joint[7] = {0}, jointd[7] = {0}, jointdd[7] = {0};
14
15            // 初始化电机（7轴）
16            for (size_t i = 0; i < 30; i++)
17            {
18                  for (int j = 0; j < 7; j++)
19                  {
20                        rb.Motors[j]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
21                  }
22                  rb.motor_send();
23                  r.sleep();
24            }
25
26            // 获取所有关节状态
27            for (int j = 0; j < 7; j++)
28            {
29                  joint_motor[j] = *rb.Motors[j]->get_current_motor_state();
30                  joint[j] = joint_direction[j] * joint_motor[j].position;
31                  jointd[j] = joint_direction[j] * joint_motor[j].velocity;
32            }
33
34            while (1)
35            {
36                  // 计算重力补偿力矩
37                  tor = DofModel.Get_Tor(joint, jointd, jointdd, model, data);
38
39                  // // 打印所有关节力矩
40                  // ROS_INFO("Torques: %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
41                  //          tor[0], tor[1], tor[2], tor[3], tor[4], tor[5], tor[6]);
42
43                  // 发送补偿力矩（应用方向系数）
44                  rb.Motors[0]->fresh_cmd(0, 0, tor[0], 0, 0);
45                  rb.Motors[1]->fresh_cmd(0, 0, tor[1], 0, 0);
46                  rb.Motors[2]->fresh_cmd(0, 0, tor[2], 0, 0);
47                  rb.Motors[3]->fresh_cmd(0, 0, -tor[3], 0, 0);
48                  rb.Motors[4]->fresh_cmd(0, 0, tor[4], 0, 0);
49                  rb.Motors[5]->fresh_cmd(0, 0, tor[5], 0, 0);
50                  rb.Motors[6]->fresh_cmd(0, 0, 0, 0, 0);
51
52                  rb.motor_send();
53
54                  // 更新关节状态
55                  for (int j = 0; j < 7; j++)
56                  {
57                        joint_motor[j] = *rb.Motors[j]->get_current_motor_state();
58                        joint[j] = joint_direction[j] * joint_motor[j].position;
59                        jointd[j] = joint_direction[j] * joint_motor[j].velocity;
60                  }
61
62                  // // 打印所有关节位置
63                  ROS_INFO("Positions: %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
64                           joint[0], joint[1], joint[2], joint[3], joint[4], joint[5], joint[6]);
65
66                  r.sleep();
67            }
68            cout << "7轴测试完毕" << endl;
69      }

这里特别需要注意的是，电机旋转轴的方向，和urdf中定义的方向一定要一样，这里加上的方向系数，可以结合urdf中的rviz仿真验证，一开始因为每调整好方向，想着可以试出来浪费了很多时间

1// 各关节方向系数（由用户自定义，1.0或-1.0）
2const float joint_direction[7] = {-1.0, -1.0, -1.0, 1.0, -1.0, -1.0, -1.0}; // 请根据实际情况修改

获取动力补偿力矩这里需要注意重力的方向设置同样要和urdf中一样

1//获取重力补偿力矩
2    VectorXf Get_Tor(
3        const double q[7], const double dq[7], const double ddq[7],
4        pinocchio::Model &model, pinocchio::Data &data)
5    {
6        // 1. 设置重力方向（X轴方向）
7        model.gravity.linear() << 9.81, 0, 0;
8
9        // 2. 将输入数组转换为 Eigen::VectorXd
10        VectorXd q_vec = Eigen::Map<const Eigen::VectorXd>(q, 7);
11        VectorXd v_vec = Eigen::Map<const Eigen::VectorXd>(dq, 7);
12        VectorXd a_vec = Eigen::Map<const Eigen::VectorXd>(ddq, 7);
13
14        // 3. 检查模型维度是否匹配
15        assert(model.nq == 7 && "Model dimension doesn't match input size!");
16
17        // 4. 计算逆动力学（RNEA）
18        VectorXd tau = pinocchio::rnea(model, data, q_vec, v_vec, a_vec);
19
20        // 5. 转换为 float 并返回
21        return tau.cast<float>();
22    }

测试效果

补偿的效果挺好的

[IMG_0783.MOV]

b机械臂阻抗控制

机械臂阻抗控制

原理分析

核心公式

实现步骤

二轴机械臂阻抗控制

机械参数建立

DH参数建立

旋转矩阵

质量和质心

惯性张量

正逆运动学

皮诺曹动力学库

Eigen库

效果

七轴机械臂阻抗控制

机械参数建立

质量和质心

惯性张量

重力补偿调试

代码编写

测试效果

机械臂阻抗控制