一、介绍

本篇主要介绍四足机器人腿部结构设计，以及在webots仿真环境中的简单控制。

目前主流的腿部结构有曲柄滑块（spot），曲柄摇杆（宇数科技的），电机直驱（anymal），其余的还有液压驱动等就不展开介绍了。下面简单介绍一下不同方案的优缺点。

•       在这么多种驱动方案中，电机直驱在控制上可以说是最为直接的，因为不需要额外计算传动比，但是也带来一个弊端，关节的最大扭矩完全由电机的最大输出扭矩决定。并且由于电机安装在机器人腿上，使得其运动惯量加大，不适合高速运动。
•       而曲柄滑块，曲柄摇杆等间接驱动的方案，可以使得腿部更加轻盈，实现更加高速的运动，缺点就在于，无法像直驱那样提供精准的控制，毕竟传动部件越多，对装配，控制的要求也越高。并且控制系统给出各关节控制力矩之后，还需要额外的去计算实际电机输出扭矩，增加了算法的负责程度。

总的来说，各种方案之间各有利弊，没有好坏之分，只有适不适用。而在本篇当中，将会介绍曲柄滑块方案，即通过丝杆电机带动滑块从而实现腿关节的运动。

二、丝杆传动计算

首先我们定义以下符号：

• $P_h$：丝杆导程
• $F_a$：推力
• $T$：电机扭矩
• $\mu$：转化效率
• $S$：滑台位移
• qm：电机角度

对于位置关系，我们有：

对于速度关系：

对于加速度关系：

而关于动力学部分，我们通常计算其扭矩关系，根据能量守恒定律，有

因此，当推动滑块丝杆收到的扭矩为：

当电机带动丝杆转动，对滑块产生的推力为：

三、对心曲柄滑块

首先我们定义以下图中的符号

滑块位移与关节角度关系如下：

速度关系如下：

加速度关系如下：

力学关系（滑块推力与关节扭矩）如下：

综合上式，我们可以对丝杆-曲柄滑块进行运动学建模

首先根据几何关系，我们可以得出：

令 λ = R\L：

则：

综上，我们得到电机角度到足端位置的关系

四、偏置曲柄滑块（滑块轴线与关节位置不在同一水平线）

1、位置关系

根据上图的几何关系，我们可以得出以下公式

又因为 $\frac{1}{4}\sin^4\beta$近似于0，因此代入$\cos\beta$

综合上式，我们可以得到

如果我们不对cosβ进行近似，得到的精确解为：

通过实测，这精确解跟近似解差别几乎可以忽略，对我们的腿部控制可以说是没有影响的，但是计算量却大了不少，因此实际使用中还是更推荐使用近似值。

而对于速度和加速度，我们对上式求导可以得到以下公式：

接下来我们对其进行运动学测试，设计关节角度函数：

取$q_{max} = -30, q_{min}=-130$

$测试代码如下：$

import numpy as np
import matplotlib.pyplot as plt


R = 0.05
L = 0.1
L1 = 0.2
L2 = 0.2
e = 0.03
q_offset = np.arcsin(e/L1)
Q = 160*np.pi/180 + q_offset - np.pi
s_max = np.sqrt((R+L)**2-e**2)

def test3():
    num_steps = 100
    q_max = -80 * np.pi / 180
    q_min = -140 * np.pi / 180

    T = np.linspace(0, 2 * np.pi, num_steps)
    dt = 2*np.pi/num_steps

    q_array = (q_max - q_min) / 2 * np.sin(T) + (q_min + q_max) / 2
    q_dot_array = (q_max - q_min) / 2 * np.cos(T)
    q_ddot_array = -(q_max - q_min) / 2 * np.sin(T)

    v_pre = get_dis_dot(q_array[0], q_dot_array[0])
    s_pre = get_dis3(q_array[0])

    s_list = []
    v_list = []
    v1_list = []
    a_list = []
    a1_list = []

    for i in range(num_steps):
        s = get_dis3(q_array[i])
        v = get_dis_dot(q_array[i], q_dot_array[i])
        a = get_dis_ddot(q_array[i], q_dot_array[i], q_ddot_array[i])

        v1 = (s-s_pre)/dt
        a1 = (v-v_pre)/dt

        s_list.append(s)
        v_list.append(v)
        v1_list.append(v1)
        a_list.append(a)
        a1_list.append(a1)

        s_pre = s
        v_pre = v
        # print('\t', a)
        # print('\t', a1)
        # print('-' * 50)

    colors = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd',
              '#8c564b', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf']

    s_list = np.array(s_list)
    v_list = np.array(v_list)
    v1_list = np.array(v1_list)
    a_list = np.array(a_list)
    a1_list = np.array(a1_list)

    plt.figure()

    plt.subplot(321)
    plt.plot(T, s_list, label='s', color=colors[0])
    plt.legend()
    # plt.axis('equal')
    plt.grid()

    plt.subplot(322)
    plt.plot(T, q_array / np.pi * 180, label='q', color=colors[1])
    plt.legend()
    # plt.axis('equal')
    plt.grid()

    plt.subplot(312)
    plt.plot(T, v_list, label='v', color=colors[2])
    plt.plot(T, v_list, label='v-true', color=colors[3])
    plt.legend()
    # plt.axis('equal')
    plt.grid()

    plt.subplot(313)
    plt.plot(T, a_list, label='a', color=colors[2])
    plt.plot(T, a1_list, label='a-true', color=colors[3])
    plt.legend()
    # plt.axis('equal')
    plt.grid()

    plt.show()

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

四、仿真控制

我们利用webots进行仿真控制，控制器代码如下：

from controller import Robot
import numpy as np
import matplotlib.pyplot as plt

R = 0.05
L = 0.1
L1 = 0.2
L2 = 0.2
e = 0.03
q_offset = np.arcsin(e/L1)
Q = 150*np.pi/180 + q_offset

s_max = np.sqrt((R+L)**2-e**2)

robot = Robot()
TIME_STEP = int(robot.getBasicTimeStep())

motors = []

motor_names = ['hip motor',
               'knee linear motor']

position_sensors = []
position_sensor_names = ['hip position sensor',
                         'knee position sensor',
                         'slider position sensor']

q_pre = np.array([0, 0])


def get_data():
    global q_pre
    q = [0, 0]
    qd = [0, 0]
    q[0] = position_sensors[0].getValue()
    q[1] = position_sensors[1].getValue()
    # q[2] = position_sensors[2].getValue()

    qd[0] = (q[0] - q_pre[0])*1000/TIME_STEP
    qd[1] = (q[1] - q_pre[1])*1000/TIME_STEP
    # qd[2] = q[2] - q_pre[2]
    q_pre = q + np.array([0, 0])
    return np.array(q), np.array(qd)


# 逆运动学（二维）
def inverse_kinematic(x, y):
    theta2 = -np.pi + np.arccos((L1**2 + L2**2-x**2-y**2)/(2*L1*L2))
    theta1 = np.arccos((x**2+y**2+L1**2-L2**2)/(2*np.sqrt(x**2+y**2)*L1))-np.arctan2(x, abs(y))
    print('des knee angle:\t', theta2 * 180 / np.pi)
    print('des hip angle:\t', theta1 * 180 / np.pi)
    alpha = Q + theta2
    # print('alpha:\t', alpha * 180 / np.pi)
    s = s_max - L - R * np.cos(alpha) + 0.5 * (R * np.sin(alpha) - e) ** 2 / L
    return [theta1, s]


def forward_kinematic(angle1, angle2):
    x = -L1 * np.sin(angle1) - L2 * np.sin(angle1+angle2)
    z = -L1 * np.cos(angle1) - L2 * np.cos(angle1+angle2)
    return np.array([x, z])


def jacobian(theta1, theta2):
    J = np.zeros([2, 2])
    J[0, 0] = -L1 * np.cos(theta1) - L2 * np.cos(theta1 + theta2)
    J[0, 1] = -L2 * np.cos(theta1 + theta2)
    J[1, 0] = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
    J[1, 1] = L2 * np.sin(theta1 + theta2)
    return J


def dynamic(force):
    theta1 = position_sensors[0].getValue()
    theta2 = position_sensors[1].getValue()
    tau = np.matmul(jacobian(theta1, theta2).transpose(), force)
    print('tau: ', tau)
    alpha = Q + theta2

    c_alpha = np.cos(alpha)
    s_alpha = np.sin(alpha)
    s_beta = (R * s_alpha - e) / L
    c_beta = 1 - 0.5 * s_beta ** 2

    m1 = R * (s_alpha*c_beta + c_alpha*s_beta)
    P = tau[1] * c_beta / m1
    return [tau[0], P]


def stand(height):
    kp = np.array([[10, 0], [0, 1500]])
    kd = np.array([[1, 0], [0, 500]])
    p_des = np.array([0, -height])
    p_dot_des = np.array([0, 0])
    q, qd = get_data()
    # print(q)
    # print(qd)
    p_current = forward_kinematic(q[0], q[1])
    p_dot_current = np.matmul(jacobian(q[0], q[1]), qd)
    print('p_dot_current:\t', p_dot_current)
    print('p_current:\t', p_current)
    force = np.matmul(kp, p_des-p_current) + np.matmul(kd, p_dot_des-p_dot_current)
    print('force:\t', force)
    tau = dynamic(force)
    print('command:\t', tau)
    return tau


def init():
    for i in range(len(motor_names)):
        motors.append(robot.getDevice(motor_names[i]))

    for i in range(len(position_sensor_names)):
        position_sensors.append((robot.getDevice(position_sensor_names[i])))
        position_sensors[i].enable(TIME_STEP)


# angle1, angle2 = inverse_kinematic(0, -0.5)
# motors[0].setPosition(angle1)
# motors[1].setPosition(angle2)

cnt = 0

init()
while robot.step(TIME_STEP) != -1:

    # angle1, angle2 = inverse_kinematic(0, height)
    # motors[0].setPosition(angle1)
    # motors[1].setPosition(angle2)
    # height = 0.075*np.sin(cnt) + 0.425
    tau = stand(0.2)
    # tau = [0, 100]
    motors[0].setTorque(tau[0])
    motors[1].setForce(tau[1])
    hip_angle = position_sensors[0].getValue()
    knee_angle = position_sensors[1].getValue()
    slider_pos = position_sensors[2].getValue()
    print('L_bar:\t', s_max)
    # print('S:\t', angle2)
    print('hip joint:\t', hip_angle*180/np.pi)
    print('knee joint:\t', knee_angle*180/np.pi)
    print('linear position:\t', slider_pos)
    print('-'*50)
    cnt += 0.1

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制

复制