214 lines
7.5 KiB
C++
214 lines
7.5 KiB
C++
|
||
#include "comp_vmc.hpp"
|
||
|
||
#include <tuple>
|
||
|
||
#define PI_2 1.571f
|
||
|
||
using namespace Component;
|
||
|
||
VMC::VMC(VMC::Param ¶m, float sample_freq) : param_(param) {
|
||
float dt_min = 1.0f / sample_freq;
|
||
XB_ASSERT(isfinite(dt_min));
|
||
|
||
this->Reset();
|
||
}
|
||
|
||
/*
|
||
|
||
正负极参考韭菜的菜 知乎 平衡步兵控制系统设计
|
||
|
||
VMC 机体pitch正负极 d_pitch同
|
||
|
||
/
|
||
/ 正+
|
||
/
|
||
x ---------> 0
|
||
\ 负-
|
||
\
|
||
\
|
||
|
||
phi角正负极 d_phi同
|
||
|
||
/
|
||
/ 正+
|
||
/
|
||
x ---------> 0
|
||
\ 负-
|
||
\
|
||
\
|
||
|
||
后 <---->前
|
||
/phi1-----phi4\
|
||
/ \
|
||
\ /
|
||
\ OO /
|
||
\O轮O/
|
||
OO
|
||
|
||
*/
|
||
|
||
/* 两个大腿角度 机体角度 角速度 求出虚拟腿摆角 摆角速度 虚拟腿长
|
||
* 虚拟腿长变化速度 */
|
||
std::tuple<float, float, float, float> VMC::VMCsolve(float phi1, float phi4,
|
||
float eulrPit,
|
||
float d_eulrPit,
|
||
float dt) {
|
||
static float body_pitch = 0.0f;
|
||
static float d_body_pitch = 0.0f;
|
||
body_pitch = eulrPit;
|
||
d_body_pitch = d_eulrPit;
|
||
/*点D B x y坐标 */
|
||
this->vmc_leg.YD = this->param_.leg_4 * sinf(phi4);
|
||
this->vmc_leg.YB = this->param_.leg_1 * sinf(phi1);
|
||
this->vmc_leg.XD = this->param_.hip_length + this->param_.leg_4 * cosf(phi4);
|
||
this->vmc_leg.XB = this->param_.leg_1 * cosf(phi1);
|
||
/*BD长度*/
|
||
this->vmc_leg.lBD = sqrtf((this->vmc_leg.XD - this->vmc_leg.XB) *
|
||
(this->vmc_leg.XD - this->vmc_leg.XB) +
|
||
(this->vmc_leg.YD - this->vmc_leg.YB) *
|
||
(this->vmc_leg.YD - this->vmc_leg.YB));
|
||
|
||
this->vmc_leg.A0 =
|
||
2 * this->param_.leg_2 * (this->vmc_leg.XD - this->vmc_leg.XB);
|
||
this->vmc_leg.B0 =
|
||
2 * this->param_.leg_2 * (this->vmc_leg.YD - this->vmc_leg.YB);
|
||
this->vmc_leg.C0 = this->param_.leg_2 * this->param_.leg_2 +
|
||
this->vmc_leg.lBD * this->vmc_leg.lBD -
|
||
this->param_.leg_3 * this->param_.leg_3;
|
||
this->vmc_leg.phi2 =
|
||
2 *
|
||
atan2f((this->vmc_leg.B0 + sqrtf(this->vmc_leg.A0 * this->vmc_leg.A0 +
|
||
this->vmc_leg.B0 * this->vmc_leg.B0 -
|
||
this->vmc_leg.C0 * this->vmc_leg.C0)),
|
||
this->vmc_leg.A0 + this->vmc_leg.C0);
|
||
this->vmc_leg.phi3 =
|
||
atan2f(this->vmc_leg.YB - this->vmc_leg.YD +
|
||
this->param_.leg_2 * sinf(this->vmc_leg.phi2),
|
||
this->vmc_leg.XB - this->vmc_leg.XD +
|
||
this->param_.leg_2 * cosf(this->vmc_leg.phi2));
|
||
/*点C x y坐标 */
|
||
this->vmc_leg.XC = this->param_.leg_1 * cosf(phi1) +
|
||
this->param_.leg_2 * cosf(this->vmc_leg.phi2);
|
||
this->vmc_leg.YC = this->param_.leg_1 * sinf(phi1) +
|
||
this->param_.leg_2 * sinf(this->vmc_leg.phi2);
|
||
/*点C 极坐标 */
|
||
this->vmc_leg.L0 =
|
||
sqrtf((this->vmc_leg.XC - this->param_.hip_length / 2.0f) *
|
||
(this->vmc_leg.XC - this->param_.hip_length / 2.0f) +
|
||
this->vmc_leg.YC * this->vmc_leg.YC);
|
||
this->vmc_leg.phi0 = atan2f(
|
||
this->vmc_leg.YC, (this->vmc_leg.XC - this->param_.hip_length / 2.0f));
|
||
|
||
this->vmc_leg.alpha = PI_2 - this->vmc_leg.phi0;
|
||
|
||
this->vmc_leg.d_phi0 = (this->vmc_leg.phi0 - this->vmc_leg.last_phi0) / dt;
|
||
this->vmc_leg.d_alpha = 0.0f - this->vmc_leg.d_phi0;
|
||
/*虚拟腿 摆角theta 摆角速度d_theta */
|
||
this->vmc_leg.theta = PI_2 + body_pitch - this->vmc_leg.phi0;
|
||
this->vmc_leg.d_theta = (-d_body_pitch - this->vmc_leg.d_phi0);
|
||
|
||
this->vmc_leg.last_phi0 = this->vmc_leg.phi0;
|
||
/*虚拟腿 腿长L0 腿长变化速度d_L0 */
|
||
this->vmc_leg.d_L0 = (this->vmc_leg.L0 - this->vmc_leg.last_L0) / dt;
|
||
this->vmc_leg.dd_L0 = (this->vmc_leg.d_L0 - this->vmc_leg.last_d_L0) / dt;
|
||
|
||
this->vmc_leg.last_d_L0 = this->vmc_leg.d_L0;
|
||
this->vmc_leg.last_L0 = this->vmc_leg.L0;
|
||
|
||
this->vmc_leg.dd_theta =
|
||
(this->vmc_leg.d_theta - this->vmc_leg.last_d_theta) / dt;
|
||
this->vmc_leg.last_d_theta = this->vmc_leg.d_theta;
|
||
|
||
return std::make_tuple(vmc_leg.L0, vmc_leg.d_L0, vmc_leg.theta,
|
||
vmc_leg.d_theta);
|
||
}
|
||
|
||
/* 两个大腿角度 期望腿支持力 期望腿摆力矩 求出两个关节输出力矩 */
|
||
std::tuple<float, float> VMC::VMCinserve(float phi1, float phi4, float Tp,
|
||
float F0) {
|
||
/*jacobian矩阵计算*/
|
||
this->vmc_leg.j11 =
|
||
(this->param_.leg_1 * sinf(this->vmc_leg.phi0 - this->vmc_leg.phi3) *
|
||
sinf(phi1 - this->vmc_leg.phi2)) /
|
||
sinf(this->vmc_leg.phi3 - this->vmc_leg.phi2);
|
||
this->vmc_leg.j12 =
|
||
(this->param_.leg_1 * cosf(this->vmc_leg.phi0 - this->vmc_leg.phi3) *
|
||
sinf(phi1 - this->vmc_leg.phi2)) /
|
||
(this->vmc_leg.L0 * sinf(this->vmc_leg.phi3 - this->vmc_leg.phi2));
|
||
this->vmc_leg.j21 =
|
||
(this->param_.leg_4 * sinf(this->vmc_leg.phi0 - this->vmc_leg.phi2) *
|
||
sinf(this->vmc_leg.phi3 - phi4)) /
|
||
sinf(this->vmc_leg.phi3 - this->vmc_leg.phi2);
|
||
this->vmc_leg.j22 =
|
||
(this->param_.leg_4 * cosf(this->vmc_leg.phi0 - this->vmc_leg.phi2) *
|
||
sinf(this->vmc_leg.phi3 - phi4)) /
|
||
(this->vmc_leg.L0 * sinf(this->vmc_leg.phi3 - this->vmc_leg.phi2));
|
||
/*得到前髋关节的输出轴期望力矩，F0为五连杆机构末端沿腿的推力*/
|
||
this->vmc_leg.torque_set[0] = this->vmc_leg.j11 * F0 + this->vmc_leg.j12 * Tp;
|
||
/*得到后髋关节的输出轴期望力矩，Tp为虚拟腿摆力矩的力矩*/
|
||
this->vmc_leg.torque_set[1] = this->vmc_leg.j21 * F0 + this->vmc_leg.j22 * Tp;
|
||
|
||
return std::make_tuple(this->vmc_leg.torque_set[0],
|
||
this->vmc_leg.torque_set[1]);
|
||
}
|
||
/* 用到了前两个函数解算算出来的变量 尽量放在前两个函数之后 */
|
||
float VMC::GndDetector(float Tp, float F0) {
|
||
vmc_leg.Fn =
|
||
F0 * cosf(vmc_leg.theta) + Tp * sinf(vmc_leg.theta) / vmc_leg.L0 + 6; //
|
||
// 腿部机构的力+轮子重力，这里忽略了轮子质量*驱动轮竖直方向运动加速度
|
||
|
||
// vmc_leg.Fn =
|
||
// F0 * cosf(vmc_leg.theta) + Tp * sinf(vmc_leg.theta) / vmc_leg.L0 +
|
||
// wheel_mess *
|
||
// (z_accl - vmc_leg.dd_L0 * cosf(vmc_leg.theta) +
|
||
// 2.0f * vmc_leg.d_L0 * vmc_leg.d_theta * sinf(vmc_leg.theta) +
|
||
// vmc_leg.L0 * vmc_leg.dd_theta * sinf(vmc_leg.theta) +
|
||
// vmc_leg.L0 * vmc_leg.d_theta * vmc_leg.d_theta *
|
||
// cosf(vmc_leg.theta));
|
||
return vmc_leg.Fn;
|
||
}
|
||
|
||
/* 计算拟合函数结果 */
|
||
float VMC::LQR_K_calc(float *coe, float len) {
|
||
return coe[0] * len * len * len + coe[1] * len * len + coe[2] * len + coe[3];
|
||
}
|
||
|
||
/* 变量刷新 */
|
||
void VMC::Reset() {
|
||
vmc_leg.L0 = 0;
|
||
vmc_leg.phi0 = 0;
|
||
vmc_leg.alpha = 0;
|
||
vmc_leg.d_alpha = 0;
|
||
|
||
vmc_leg.lBD = 0;
|
||
|
||
vmc_leg.d_phi0 = 0;
|
||
vmc_leg.last_phi0 = 0;
|
||
|
||
vmc_leg.A0 = 0;
|
||
vmc_leg.B0 = 0;
|
||
vmc_leg.C0 = 0;
|
||
vmc_leg.phi2 = 0;
|
||
vmc_leg.phi3 = 0;
|
||
|
||
vmc_leg.j11 = 0;
|
||
vmc_leg.j12 = 0;
|
||
vmc_leg.j21 = 0;
|
||
vmc_leg.j22 = 0;
|
||
vmc_leg.torque_set[0] = 0;
|
||
vmc_leg.torque_set[1] = 0;
|
||
|
||
vmc_leg.theta = 0;
|
||
vmc_leg.d_theta = 0;
|
||
vmc_leg.last_d_theta = 0;
|
||
vmc_leg.dd_theta = 0;
|
||
|
||
vmc_leg.d_L0 = 0;
|
||
vmc_leg.dd_L0 = 0;
|
||
vmc_leg.last_L0 = 0;
|
||
vmc_leg.last_d_L0 = 0;
|
||
vmc_leg.first_flag = 0;
|
||
vmc_leg.leg_flag = 0;
|
||
}
|