利用几何法对狗的的一条腿进行逆运动学的求解，因为有时候会有多解所以要分情况进行讨论 具体的算法可以看我以前机械臂逆运动学的博客，原理都一样 下面是一些定义函数和设置初值的程序图
#include <Arduino.h> #include <WiFi.h> #define DJ0_PWM 13 #define DJ1_PWM 12 #define DJ2_PWM 14 double x,z,BD,xB,zB,L1 = 57,L2 = 50,L3 = 61; double duty0; // 占空比变量 double duty1; // 占空比变量 double duty2; // 占空比变量 int dutyCycle0; // 设置占空比 int dutyCycle1; // 设置占空比 int dutyCycle2; // 设置占空比 double Initial_Alpha = 90; //设置角度 double Initial_Beta = 90; //设置角度 int freq = 50; int freq0 = freq; // 频率 int channel0 = 0; // 通道0，共16个通道，0~15 int resolution0 = 10; // 分辨率，取值0~20，duty最大取值为2^resolution-1 int freq1 = freq; // 频率 int channel1 = 1; // 通道1，共16个通道，0~15 int resolution1 = 10; // 分辨率，取值0~20，duty最大取值为2^resolution-1 int freq2 = freq; // 频率 int channel2 = 2; // 通道0，共16个通道，0~15 int resolution2 = 10; // 分辨率，取值0~20，duty最大取值为2^resolution-1 struct ForwardKinematics //建立一个结构体用来存储计算后的角度值 { double Alpha; //角度α double Beta; //角度β }h; void ledc_Initialization() //ledc初始化程序 { ledcSetup(channel0, freq0, resolution0); // 设置通道0 ledcSetup(channel1, freq1, resolution1); // 设置通道1 ledcSetup(channel2, freq2, resolution2); // 设置通道2 ledcAttachPin(DJ0_PWM, channel0); // 将通道0与引脚13连接 ledcAttachPin(DJ1_PWM, channel1); // 将通道1与引脚12连接 ledcAttachPin(DJ2_PWM, channel2); // 将通道2与引脚14连接 } void ledc_dutycycle() //比较角度设置占空比 { duty0 = 73.3; // 设置初始占空比 duty1 = 102.4; // 设置初始占空比 duty2 = 76.8; // 设置初始占空比 Initial_Alpha = 90; //设置初始角度 Initial_Beta = 90; //设置初始角度 if(h.Alpha>Initial_Alpha) { duty1=duty1-(h.Alpha-Initial_Alpha)*0.71; } if(h.Alpha<Initial_Alpha) { duty1=duty1+(Initial_Alpha-h.Alpha)*0.71; } if(h.Beta>Initial_Beta) { duty2=duty2+(h.Beta-Initial_Beta)*0.71; } if(h.Beta<Initial_Beta) { duty2=duty2-(Initial_Beta-h.Beta)*0.71; } dutyCycle0 = duty0; // 设置占空比 dutyCycle1 = duty1; // 设置占空比 dutyCycle2 = duty2; // 设置占空比 Initial_Alpha = h.Alpha; //设置角度 Initial_Beta = h.Beta; //设置角度 } void setup() { ledc_Initialization(); // ledc初始化程序 Inverse_kinematics(0,111); //逆运动学计算公式 ledc_dutycycle(); //比较角度设置占空比 } void loop() { ledcWrite(channel0, dutyCycle0); // 输出PWM,dutyCycle为占空比 ledcWrite(channel1, dutyCycle1); // 输出PWM,dutyCycle为占空比 ledcWrite(channel2, dutyCycle2); // 输出PWM,dutyCycle为占空比 delay(5); }通过求得的逆运动学公式计算角度值
void Inverse_kinematics(double x,double z) //逆运动学计算公式 { xB = 0; zB = 1.732/2*L1; if(x<0) { x = -x; BD = sqrt(x*x+(z-zB)*(z-zB)); h.Alpha = 60+acos((x*x+BD*BD-(z-zB)*(z-zB))/(2*x*BD))*180/PI-acos((L2*L2+BD*BD-L3*L3)/(2*L2*BD))*180/PI; h.Beta = acos((L2*L2+L3*L3-BD*BD)/(2*L2*L3))*180/PI; }else if(x>0) { BD = sqrt(x*x+(z-zB)*(z-zB)); h.Alpha = 60+180-acos((L2*L2+BD*BD-L3*L3)/(2*L2*BD))*180/PI-acos((x*x+BD*BD-(z-zB)*(z-zB))/(2*x*BD))*180/PI; h.Beta = acos((L2*L2+L3*L3-BD*BD)/(2*L2*L3))*180/PI; }else { BD = z-zB; h.Alpha = 150-acos((L2*L2+BD*BD-L3*L3)/(2*L2*BD))*180/PI; h.Beta = acos((L2*L2+L3*L3-BD*BD)/(2*L2*L3))*180/PI; } }将之前测得的初值与逆运动学计算的数值进行对比
void ledc_dutycycle() //比较角度设置占空比 { duty0 = 73.3; // 设置初始占空比 duty1 = 102.4; // 设置初始占空比 duty2 = 76.8; // 设置初始占空比 Initial_Alpha = 90; //设置初始角度 Initial_Beta = 120; //设置初始角度 if(h.Alpha>Initial_Alpha) { duty1=duty1-(h.Alpha-Initial_Alpha)*0.71; } if(h.Alpha<Initial_Alpha) { duty1=duty1+(Initial_Alpha-h.Alpha)*0.71; } if(h.Beta>Initial_Beta) { duty2=duty2+(h.Beta-Initial_Beta)*0.71; } if(h.Beta<Initial_Beta) { duty2=duty2-(Initial_Beta-h.Beta)*0.71; } dutyCycle0 = duty0; // 设置占空比 dutyCycle1 = duty1; // 设置占空比 dutyCycle2 = duty2; // 设置占空比 Initial_Alpha = h.Alpha; //设置角度 Initial_Beta = h.Beta; //设置角度 }舵机通过比较后的值进行调整，实现输入坐标来控制腿的位置。
