我的狗子-业余四足机器人笔记（2）-单腿逆解（几何法）

引入

之前不知道从哪里看到的，反解的三种方法：几何法，解析法，数值法
（1）几何法就是运用一系列的三角公式求解
（2）解析法：比如DH反解（我线代垃圾，待备研的同时好好学一下线代），想学习的话去coursera的robotics课程。b站有搬运的运动学片段：搜索关键字：台大机器人学，林沛群。
（3）数值法：需要高性能处理单元，类似迭代数值试凑

坐标系

狗子的全部解算都是基于这个坐标系方向

步骤

参数：
（1）足端坐标：X,Y,Z
（2）三杆件夹角（即反解的目标值）：∠A（髋关节）,∠B（大腿关节）,∠C（小腿关节）
（3）3连杆长度：D,E,F
（4）中间变量：G,H,I。∠J,∠K
1. 髋关节角度求解（∠A）：
![在这里插入图片描述](https://img-blog.csdnimg.cn/20200703002911621.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzNTM3MjQz,size_16,color_FFFFFF,t_70


2. 大腿关节求解（∠B）：




3. 小腿关节求解（∠C）：

综上：已知足端坐标（X,Y,Z），便可以求得三个夹角∠A,∠B,∠C.

示例代码：

#include "math.h"   
                         
float A,B,C,J,K;    
float G,H,I,D=32,E=46,F=62;                     //狗腿参数
float Pos_cal_A(float X,float Y,float Z)        //髋关节角度反解函数
{                   
    A=atan(Y/Z)*(180/3.1415);
    return -A;                       //符号为实验所得

}       
float Pos_cal_B(float X,float Y,float Z)        //大腿关节角度反解函数
{
    G=sqrt(X*X+Z*Z+Y*Y);
    H=sqrt(G*G-X*X)-D;
    J=abs(atan(X/H)*(180/3.1415));
    I=sqrt(H*H+X*X);
    K=abs(acos((E*E+I*I-F*F)/(2*E*I))*(180/3.1415));
    if(X>0)
        B=K-J;
    else
        B=K+J;	

    return B;
}   

float Pos_cal_C(float X,float Y,float Z)         //小腿关节角度反解函数
{
    C=acos((E*E+F*F-I*I)/(2*E*F))*(180/3.1415);
    return -(180-C);                              //符号为实验所得

结语：

注意自己狗子电机的排布方式是否一样，否则需要对公式稍作修改。X,Y,Z坐标是相对于髋关节电机轴轴心的，不是相对于整体的中心