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

本篇为第三部分，末端执行器笛卡尔空间的力位混合控制方法。

《机器人学导论》第三版（John Craig等）的力位混合控制写的比较抽象，结合《现代机器人学》第四版（凯文 林奇等），继续总结机械臂控制方法。并以UR5机械臂为例，在MUJOCO仿真中执行一个力位混合控制。复用前面位置控制的代码。

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

4. 笛卡尔空间的力位混合控制

(1) 静止状态下的力控

一般情况下，在力控任务期间，机器人运动缓慢或不运动，因此忽略速度和加速度，机器人的动力学模型是

�=�(�)+������

考虑具有前馈和重力补偿的PI力控制器

�=�(�)+��[��+��(��−�)+��∫(��−�)��]

要求在施加力的方向上需要有施力对象，否则机械臂将持续运动至不受控。

控制器的实现为

class RobotForceController:
    def __init__(self, kp, ki, robot_model, ee_body_name, ee_point_on_body):
        self.kp = kp
        self.ki = ki
        self.robot_model = robot_model
        self.ee_body_name = ee_body_name
        self.ee_point_on_body = ee_point_on_body
        self.F_err_int = np.zeros(6)

    def update(self, q, qd, F_d, F):
        tau_grav = self.robot_model.gravity_torque(q)
        jac = self.robot_model.jacobian(q, self.ee_body_name, self.ee_point_on_body)
        self.F_err_int += F_d - F
        tau = tau_grav + jac.T @ (F_d + self.kp * (F_d - F) + self.ki * self.F_err_int)
        return tau

在xml模型中增加一个固定的墙壁（加在worldbody下）

<body name="obstacle" pos="0.805 0 1.2" quat="1 0 0 0">
    <geom pos="0 0 0" quat="1 0 0 0" type="box" rgba="0.7 0.7 0.7 0.5" size="0.005 1 1" condim="1"/>
</body>

增加一个sphere作为末端执行器（加在wrist_3_link的body下）

<geom name="end_effector" pos="0 0 0.025" quat="1 0 0 0" type="sphere" rgba="1 0 0 1" size="0.025" condim="1"/>

注意到这里condim=”1″是先假设接触没有摩擦力的情况。

在mujoco中仿真机械臂对一个墙壁施加正弦变化的力：

import numpy as np
import mujoco
import mujoco.viewer

mjcf_file = "ur5/ur5.xml"
model = mujoco.MjModel.from_xml_path(mjcf_file)
data = mujoco.MjData(model)
dt = model.opt.timestep
dof = model.nv

robot_model = RobotModel(mjcf_file)

# Initialize joint states
target = np.eye(4)
target[:3, :3] = rotation
target[:3, 3] = pos[0]
q0 = robot_model.inverse_kinematics(
    target, "wrist_3_link", point_at_body=np.array([0, 0, 0.05]), smooth=False)
data.qpos = q0
data.qvel = np.zeros(dof)

# Initialize controller
kp = 0.05
ki = 0.05
robot_controller = RobotForceController(
    kp, ki, robot_model, "wrist_3_link", np.array([0, 0, 0.05])
)

F_dir = 20.0 + 10.0 * np.sin(t)
step = 0
with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running() and step < len(t):
        f, p = get_contact_force(model, data, "end_effector")
        show_contact_force(viewer, f, p)
        F = np.zeros(6)
        if not len(f) == 0:
            F = np.array([f[0][0], 0.0, 0.0, 0.0, 0.0, 0.0])

        F_d = np.array([F_dir[step], 0.0, 0.0, 0.0, 0.0, 0.0])
        data.ctrl = robot_controller.update(data.qpos, data.qvel, F_d, F)
        mujoco.mj_step(model, data)

        viewer.sync()
        step = step + 1

其中，用到mujoco获取接触力的函数：

def get_contact_force(model, data, geom_name):
    # 获取作用在body_name上的作用力
    forces = []
    positions = []
    if data.ncon == 0:
        return forces, positions

    geom_id = model.geom(geom_name).id
    for i in range(data.ncon):
        if data.contact[i].geom1 == geom_id:
            p = data.contact[i].pos
            efc_address = data.contact[i].efc_address
            dim = data.contact[i].dim
            contact_force = data.efc_force[efc_address : (efc_address + dim)]
            if dim == 1:
                f = contact_force * data.contact[i].frame[:3]
            else:
                f = data.contact[i].frame.reshape(3, 3).T @ contact_force[0:3]
            forces.append(f)
            positions.append(p)
        if data.contact[i].geom2 == geom_id:
            p = data.contact[i].pos
            efc_address = data.contact[i].efc_address
            dim = data.contact[i].dim
            contact_force = data.efc_force[efc_address : (efc_address + dim)]
            if dim == 1:
                f = contact_force * data.contact[i].frame[:3]
            else:
                f = data.contact[i].frame.reshape(3, 3).T @ contact_force[0:3]
            forces.append(-f)
            positions.append(p)

    return np.array(forces), np.array(positions)

以及在mujoco中可视化作用力（用一条线段表示力的大小和方向）

def show_contact_force(viewer, f, p, scale=0.01):
    viewer.user_scn.ngeom = 0
    num = 0
    for i in range(len(f)):
        mujoco.mjv_initGeom(
            viewer.user_scn.geoms[num],
            mujoco.mjtGeom.mjGEOM_LINE,
            np.zeros(3),
            np.zeros(3),
            np.zeros(9),
            np.array([0.0, 0.0, 1.0, 1.0]),
        )
        start_point = p[i]
        end_point = p[i] + scale * f[i]
        mujoco.mjv_connector(
            viewer.user_scn.geoms[num],
            mujoco.mjtGeom.mjGEOM_LINE,
            2.0,
            start_point,
            end_point,
        )
        num += 1
    viewer.user_scn.ngeom = num

仿真效果如下图所示（机械臂静止，对墙壁施加正弦变化的力）

(2) 力位混合控制

很多情况下，机械臂也需要一边运动一边施加力，就需要力位混合控制。

机械臂末端一般有6个自由度，一般情况下每个方向上同时要么是位置控，要么是力控。

采用对角矩阵S描述位置控制的空间方向，如果某个方向是位置控，则S对应的对角元素是1，否则是0。那么对应的力控的空间方向对角矩阵是I-S。

在位置控制的方向上，采用前面的笛卡尔空间位置控制方法；而在力控制的方向上，添加前面的力控制器。

首先考虑一个简单的情况，即速度较小时，可以只考虑重力前馈。控制率为

�=��{�{��[��−��(�)]+��(�˙�−��˙)}+(�−�)[��(��−�)+��∫(��−�)��]}+�(�)

控制器的具体实现为：

class RobotForcePositionController:
    def __init__(self, kp_pos, kd_pos, kp_frc, ki_frc, robot_model, ee_body_name, ee_point_on_body,):
        self.kp_pos = kp_pos
        self.kd_pos = kd_pos
        self.kp_frc = kp_frc
        self.ki_frc = ki_frc
        self.robot_model = robot_model
        self.ee_body_name = ee_body_name
        self.ee_point_on_body = ee_point_on_body
        self.F_err_int = np.zeros(6)

    def update(self, q, qd, pos_ref, rotmat_ref, vel_ref, angvel_ref, F, F_d, S):
        tau_grav = self.robot_model.gravity_torque(q)
        jac = self.robot_model.jacobian(q, self.ee_body_name, self.ee_point_on_body)
        self.F_err_int += F_d - F
        tau = tau_grav + jac.T @ (
            (np.eye(6) - S) @ (self.kp_frc * (F_d - F) + self.ki_frc * self.F_err_int)
        )
        pos, rotmat = self.robot_model.forward_kinematics(
            q, self.ee_body_name, self.ee_point_on_body
        )
        p = pos_ref - pos
        R = rotmat.T @ rotmat_ref
        w = rotmat @ Rotation.from_matrix(R).as_rotvec()
        pos_err = np.hstack((p, w))
        ee_vel_ref = np.hstack((vel_ref, angvel_ref))
        vel_err = ee_vel_ref - jac @ qd
        tau += jac.T @ (S @ (self.kp_pos * pos_err + self.kd_pos * vel_err))
        return tau

接下来，首先考虑一个任务，末端接触墙壁，执行一个圆形轨迹，并对墙壁持续输出20N的力。这里的力控制是x方向，其他5个方向是位置控制。实现为：

import numpy as np
import mujoco
import mujoco.viewer
from scipy.spatial.transform import Rotation

mjcf_file = "ur5/ur5.xml"
model = mujoco.MjModel.from_xml_path(mjcf_file)
data = mujoco.MjData(model)
dt = model.opt.timestep
dof = model.nv

robot_model = RobotModel("ur5/ur5.xml")

center = np.array([0.8, 0.0, 1.2])
radius = 0.1
orientation = Rotation.from_rotvec([0.0, np.pi / 2, 0.0]).as_matrix()
period = 5.0
t, pos, vel, acc = circular_trajectory(center, radius, orientation, dt, period)

# Initialize joint states
target = np.eye(4)
target[:3, :3] = rotation
target[:3, 3] = pos[0]
q0 = robot_model.inverse_kinematics(
    target, "wrist_3_link", point_at_body=np.array([0, 0, 0.05]), smooth=False)
data.qpos = q0
data.qvel = np.zeros(dof)

# Initialize controller
kp_pos = np.array([2000.0, 2000.0, 2000.0, 100.0, 100.0, 100.0])
kd_pos = np.array([20.0, 20.0, 20.0, 1.0, 1.0, 1.0])
kp_frc = np.ones(6) * 0.05
ki_frc = np.ones(6) * 0.05
robot_controller = RobotForcePositionController(
    kp_pos, kd_pos, kp_frc, ki_frc, robot_model, "wrist_3_link", np.array([0, 0, 0.05])
)

S = np.eye(6)
S[0, 0] = 0.0

step = 0
with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running() and step < len(t):
        f, p = get_contact_force(model, data, "end_effector")
        show_contact_force(viewer, f, p)

        F = np.zeros(6)
        if not len(f) == 0:
            F = np.array([f[0][0], 0.0, 0.0, 0.0, 0.0, 0.0])
        F_d = np.array([20.0, 0.0, 0.0, 0.0, 0.0, 0.0])
        data.ctrl = robot_controller.update(data.qpos, data.qvel, pos[step], orientation, vel[step], np.zeros(3), F, F_d, S)
        mujoco.mj_step(model, data)

        viewer.sync()
        step = step + 1

仿真结果如下：

动图封面

力和位置的跟踪效果：

既然是力位混合控制，那么可以想象到另一种情况，即力控方向上，如果障碍物在移动时，机械臂也随之移动并施加期望的力。

首先，在xml模型中增加一个joint来控制墙壁的移动

<body name="obstacle" pos="0.805 0 1.2" quat="1 0 0 0">
    <geom pos="0 0 0" quat="1 0 0 0" type="box" rgba="0.7 0.7 0.7 0.5" size="0.005 1 1" condim="1"/>
    <joint name="joint_barrier" type="slide" pos="0 0 0" axis="1 0 0" range="-10 10"/>
</body>

并增加actuator来控制运动

<position name="joint_barrier" joint="joint_barrier" ctrllimited="false" kp="1000000" kv="10000"/>

增加了一个自由度后，系统的自由度发生变化，在这里，为了保证机械臂的运动学模型正确，用没有墙壁的原模型初始化robot_model，而用有墙壁的模型执行仿真：

import numpy as np
import mujoco
import mujoco.viewer
from scipy.spatial.transform import Rotation

mjcf_file = "ur5/ur5_with_movable_obstacle.xml"
model = mujoco.MjModel.from_xml_path(mjcf_file)
data = mujoco.MjData(model)
dt = model.opt.timestep
dof = model.nv

robot_model = RobotModel("ur5/ur5.xml")

center = np.array([0.8, 0.0, 1.2])
radius = 0.1
orientation = Rotation.from_rotvec([0.0, np.pi / 2, 0.0]).as_matrix()
period = 5.0
t, pos, vel, acc = circular_trajectory(center, radius, orientation, dt, period)

# Initialize joint states
target = np.eye(4)
target[:3, :3] = rotation
target[:3, 3] = pos[0]
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, 0.0))
data.qvel = np.zeros(dof)

# Initialize controller
kp_pos = np.array([2000.0, 2000.0, 2000.0, 100.0, 100.0, 100.0])
kd_pos = np.array([20.0, 20.0, 20.0, 1.0, 1.0, 1.0])
kp_frc = np.ones(6) * 0.05
ki_frc = np.ones(6) * 0.05
robot_controller = RobotForcePositionController(
    kp_pos, kd_pos, kp_frc, ki_frc, robot_model, "wrist_3_link", np.array([0, 0, 0.05])
)

S = np.eye(6)
S[0, 0] = 0.0

p_barrier = 0.1 * np.sin(t)

step = 0
with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running() and step < len(t):
        f, p = get_contact_force(model, data, "end_effector")
        show_contact_force(viewer, f, p)

        F = np.zeros(6)
        if not len(f) == 0:
            F = np.array([f[0][0], 0.0, 0.0, 0.0, 0.0, 0.0])
        F_d = np.array([20.0, 0.0, 0.0, 0.0, 0.0, 0.0])
        data.ctrl[:6] = robot_controller.update(data.qpos[:6], data.qvel[:6], pos[step], orientation, vel[step], np.zeros(3), F, F_d, S)
        data.ctrl[-1] = p_barrier[step]
        mujoco.mj_step(model, data)

        viewer.sync()
        step = step + 1

仿真结果如下：

动图封面

这里实现的只是最简单的情况，我们可以想到，如果障碍物不是一个平面，而我们希望始终对物体施加切向的力。书中也讨论了这种情形，需要将控制表达式中的雅可比转化为末端执行器坐标下。针对这种情况，后续再做讨论。

注意到，这里的接触没有考虑摩擦。事实上即使考虑摩擦，位置跟踪也可以较好实现（感兴趣可以试试）。实际上，本文的力控方法称作直接力控，在实际应用中并不常用也并不好用。更多用到的是阻抗控制或导纳控制。可以方便地处理各个方向上不同的控制表现。将在后续继续讨论。

赏 0

猜你喜欢

灵巧手常见的传动方案都有哪些？
学习

来源：https://www.zhihu.com/question/1960299903910650751/answer/196030378238...
1周前 33
机器人动力学库哪个更好？
学习

来源：https://www.zhihu.com/question/437857717/answer/1897953312235586570 ...
2周前 22
机器人灵巧手的关节怎么设计？
学习

来源：https://www.zhihu.com/question/27944838/answer/1953754270240335673 ...
3周前 37
马斯克说，人形机器人最难的不是大脑，而是灵巧手和量产。灵巧手为何那么难？
学习

来源：https://www.zhihu.com/question/1950897949698668255/answer/196256666940...
3周前 84
1350元入门具身智能-lerobot机械臂
学习

来源：https://zhuanlan.zhihu.com/p/1895609105902704289 前段时间一直在看具...
4周前 28
机械臂完成长程多步骤的组装任务
学习

来源：https://zhuanlan.zhihu.com/p/16403276747 FMB: a Functional Manipula...
4周前 34

评论0

取消

没有账号？注册  忘记密码？