斯坦福四足机器人运动学逆解过程

斯坦福四足机器人运动学逆解过程

运动学逆解定义:

正解:已知各关节的运动参数，求末端执行器的相对参考坐标系的位姿。

逆解:根据已给定的满足工作要求的末端执行器相对参考坐标系的位置和姿态，求各关节的运动参数。

斯坦福四足机器人运动学逆解

运动学正解：知道舵/电机转角,求足端坐标

运动学逆解：知道足端坐标,求舵/电机转角

规划轨迹 ==》再通过轨迹求电机转动角度 ==》传输给电机/舵机控制模块

足端轨迹规划

摆线方程:

运动轨迹图：

结构简化

将舵机的连接点重合进行建模，可通过几何计算腿部的各个数据

计算

通过坐标（x，y）计算θ1和θ2

python求解：
1.求逆解
L1=35
L2= 80
X=30,y=100
求在此x,y条件下,求四足机器人逆解,计算
θ1和θ2是多少?

def fun(x, y, l1, l2):
    l = math.sqrt(x ** 2 + y ** 2)  # 虚拟脚长度
    psail = math.asin(x / l)  # 脚角
    fail = math.acos((l ** 2 + l1 ** 2 - l2 ** 2) / (2 * l * l1))  # 连杆分离角
    sita1 = fail - psail  # 右舵机弧度制
    sita2 = fail + psail  # 左舵机弧度制
    sita1_1 = 180 * m1 / math.pi  # 右舵机角度制
    sita2_2 = 180 * m2 / math.pi  # 左舵机角度制

    return sita1_1, sita2_2


print("sita1: %f sita2: %f" % fun(30, 100, 35, 80))

过程结果：