机械臂的控制（四）

来源：https://zhuanlan.zhihu.com/p/1967358941055915966

本篇为第四部分，机械臂末端执行器的导纳控制。

代码仓库：github.com/DrNingZhi/robot_sim

1. 导纳控制基本原理

导纳控制是根据读到的末端力，修改位置命令，使末端表现地像一个弹簧阻尼系统，适用于位置控制精度高且有末端力传感器的情况。

假定期望的末端力 ����=��¨+��˙+��

获得位置的控制量 �¨�=�−1(����−��˙−��) 其中f是力传感器读到的，x和xd是关节编码器前向运动学得到的实际值。

根据 �˙=��˙,�¨=�˙�˙+��¨

新的关节位置命令 �¨�=�−1(�¨�−�˙�˙)

2. 导纳控制的离散形式

首先，已知机械臂末端当前的期望状态、实际状态，可以在关节空间表示，也可以在笛卡尔空间表示。以及受到的外力。可以得到期望的末端加速度

�¨�=�−1[����−�(�˙−�˙�)−�(�−��)]=�−1{����−�(��˙−���˙�)−�[��(�)−��(��)]}

然后，由于 �¨�=�−1(�¨�−�˙�˙)

给机械臂发送的命令变换值:

(1) 初始时， Δ�˙����=0Δ�����=0

(2) 开始导纳控制后，每一步的叠加变化为 Δ�˙����=Δ�˙����+��⋅�¨�Δ�����=Δ�����+��⋅Δ�˙����

(3) 实际发送给机械臂的位置命令为 �˙����=�˙�+Δ�˙���������=��+Δ�����

3. 导纳控制的实现

代码复用前面的，只做导纳控制相关的修改。（这里暂时只考虑平移方向的运动，不考虑旋转）

首先，导纳控制的实现

class RobotAdmittanceController3d:
    def __init__(self, dt, M, B, K, robot_model, ee_body_name, ee_point_on_body):
        self.M = np.diag(M)
        self.B = np.diag(B)
        self.K = np.diag(K)
        self.dt = dt
        self.robot_model = robot_model
        self.ee_body_name = ee_body_name
        self.ee_point_on_body = ee_point_on_body
        self.delta_q = 0.0
        self.delta_qd = 0.0

    def update(self, f, q, qd, q_ref, qd_ref):
        if len(f) == 0:
            ff = np.zeros(3)
        else:
            ff = np.sum(f, axis=0)

        Jac = self.robot_model.jacobian(q, self.ee_body_name, self.ee_point_on_body)
        Jac_d = self.robot_model.jacobian_d(q, qd, self.ee_body_name, self.ee_point_on_body)
        Jac_ref = self.robot_model.jacobian(q_ref, self.ee_body_name, self.ee_point_on_body)
        pos, _ = self.robot_model.forward_kinematics(q, self.ee_body_name, self.ee_point_on_body)
        pos_ref, _ = self.robot_model.forward_kinematics(q_ref, self.ee_body_name, self.ee_point_on_body)
        vel = Jac @ qd
        vel_ref = Jac_ref @ qd_ref
        xdd = np.linalg.inv(self.M) @ (ff - self.B @ (vel[:3] - vel_ref[:3]) - self.K @ (pos - pos_ref))
        qdd = np.linalg.pinv(Jac[:3]) @ (xdd - (Jac_d @ qd)[:3])
        print(qdd)
        self.delta_qd += self.dt * qdd
        self.delta_q += self.dt * self.delta_qd
        return self.delta_q, self.delta_qd

其他功能复用前面的，包括关节位置控制（给较大的PD参数模拟关节高刚度位置控制），详见第一篇，接触力的读取与显示，详见第三篇。为了便于在仿真中自由交互，增加一个键盘控制的小球（并修改对应的模拟xml文件，代码不再赘述，感兴趣可以查看代码仓库）。

import numpy as np
import mujoco
import mujoco.viewer
import time
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation
from robot_sim.robot_model import RobotModel
from robot_sim.controller import RobotPDController, RobotAdmittanceController3d
from robot_sim.keyboard_controller import KeyboardController
from robot_sim.contact_force import get_contact_force, show_contact_force

sim_mjcf_file = "model/ur5/ur5_interact.xml"
rob_mjcf_file = "model/ur5/ur5.xml"

# Load model and initialize data
model = mujoco.MjModel.from_xml_path(sim_mjcf_file)
data = mujoco.MjData(model)

dt = model.opt.timestep
dof = model.nv

robot_model = RobotModel(rob_mjcf_file)

target = np.eye(4)
target[:3, :3] = Rotation.from_rotvec([0.0, np.pi / 2, 0.0]).as_matrix()
target[:3, 3] = np.array([0.8, 0.0, 1.1])
q0 = robot_model.inverse_kinematics(
    target, "wrist_3_link", point_at_body=np.array([0, 0, 0.05]), smooth=False
)
data.qpos = np.hstack((q0, np.zeros(3)))
data.qvel = np.zeros(dof)

kp = np.array([100000.0, 100000.0, 100000.0, 1000.0, 1000.0, 10.0])
kd = np.array([50.0, 50.0, 50.0, 1.0, 1.0, 0.01])
robot_controller = RobotPDController(kp, kd)
tau_ref = robot_model.gravity_torque(q0)

keyboard_controller = KeyboardController()
keyboard_controller.start()

M = np.array([1.0, 1.0, 1.0])
B = np.array([0.0, 0.0, 0.0])
K = np.array([1000.0, 1000.0, 1000.0])
robot_admittance_controller = RobotAdmittanceController3d(
    dt, M, B, K, robot_model, "wrist_3_link", np.array([0, 0, 0.05])
)

T = 1000.0
num_step = int(T / dt)
step = 0
with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running() and step < num_step:
        f, p = get_contact_force(model, data, "end_effector")
        show_contact_force(viewer, f, p)

        delta_q, delta_qd = robot_admittance_controller.update(
            f,
            data.qpos[: robot_model.dof],
            data.qvel[: robot_model.dof],
            q0,
            np.zeros(robot_model.dof),
        )

        data.ctrl[: robot_model.dof] = robot_controller.update_with_feedforward(
            data.qpos[: robot_model.dof],
            data.qvel[: robot_model.dof],
            q0 + delta_q,
            np.zeros(robot_model.dof),
            tau_ref,
        )
        data.ctrl[-3:] = keyboard_controller.state()
        mujoco.mj_step(model, data)

        viewer.sync()
        step = step + 1