改k
This commit is contained in:
parent
1f2406508d
commit
650ebc1f87
@ -159,59 +159,19 @@ Config_RobotParam_t robot_config = {
|
||||
.hip_length = 0.0f // 髋宽 (L5) (m)
|
||||
}
|
||||
},
|
||||
.lqr_gains ={
|
||||
// .k11_coeff = { -61.932040681074966f, 70.963671642086396f, -37.730841182566571f, -0.296475458388679f }, // theta
|
||||
// .k12_coeff = { -0.586710094600108f, 0.886736272521581f, -3.626144273475104f, 0.057861910518974f }, // d_theta
|
||||
// .k13_coeff = { -17.297031110481019f, 16.286794934166178f, -5.176451154477850f, -0.867260018374823f }, // x
|
||||
// .k14_coeff = { -14.893387150006664f, 14.357020815277332f, -5.244645181873441f, -0.869862096507486f}, // d_x
|
||||
// .k15_coeff = { -75.327876471378886f, 79.786699741548944f, -31.183500053811208f, 5.450635661115236f}, // phi
|
||||
// .k16_coeff = { -3.572234723237025f, 4.164905011076312f, -1.874828497771750f, 0.477913222933688f}, // d_phi
|
||||
// .k21_coeff = { 9.327090608559319f, 1.362675928111105f, -8.092777598567881f, 4.351387652359104f}, // theta
|
||||
// .k22_coeff = { 3.872517778351810f, -3.344077414726880f, 0.589693555828904f, 0.310036629174646f}, // d_theta
|
||||
// .k23_coeff = { -71.615766251649134f, 74.748309711530837f, -28.370490124006626f, 4.483586941100197f }, // x
|
||||
// .k24_coeff = { -68.751866288568962f, 71.204785013044102f, -26.812636604518396f, 4.238479323700952f }, // d_x
|
||||
// .k25_coeff = { 205.6887659080132f, -195.1219729060621f, 62.890188701113303f, 7.452313695653162f }, // phi
|
||||
// .k26_coeff = { 16.162999842656344f, -15.926932704437410f, 5.474613395300429f, -0.002856083635449f }, // d_phi
|
||||
|
||||
|
||||
// .k11_coeff = {-143.1550,156.1754,-98.5282,-11.3781}, // theta
|
||||
// .k12_coeff = {8.3196,-12.4161,-8.3805,-1.6368}, // d_theta
|
||||
// .k13_coeff = {-69.6189,68.5619,-23.3079,-4.1736}, // x
|
||||
// .k14_coeff = {-58.4944,58.2204,-22.8021,-5.2361}, // d_x
|
||||
// .k15_coeff = {-29.6753,40.9947,-20.1188,2.5142}, // phi
|
||||
// .k16_coeff = {-13.1787,15.8361,-7.4061,1.1193}, // d_phi
|
||||
// .k21_coeff = {-76.5141,124.3258,-73.4908,22.0942}, // theta
|
||||
// .k22_coeff = {-9.1845,12.7284,-5.8169,2.8659}, // d_theta
|
||||
// .k23_coeff = {-157.6244,179.9415,-77.2161,14.9075}, // x
|
||||
// .k24_coeff = {-180.5666,202.7656,-85.2869,16.4847}, // d_x
|
||||
// .k25_coeff = {14.3596,-14.2125,4.1210,24.1729}, // phi
|
||||
// .k26_coeff = {20.5751,-19.8014,6.3128,2.8044}, // d_phi
|
||||
.k11_coeff = { -2.046396270532116e+02f, 2.283572275397276e+02f, -99.051642997946473f, 2.549835715107331f }, // theta
|
||||
.k12_coeff = { 0.689999868157742f, 2.004516582917084f, -5.904779142191341f, 0.331840642644740f }, // d_theta
|
||||
.k13_coeff = { -59.058694050901643f, 59.363082825119605f, -20.603451414220757f, 1.137708603718630f }, // x
|
||||
.k14_coeff = { -64.283929532305166f, 65.138737687498519f, -23.323482861600581f, 1.257945614978053f }, // d_x
|
||||
.k15_coeff = { -15.125353856795936f, 34.329224759247211f, -22.500683150450474f, 5.036897782323629f }, // phi
|
||||
.k16_coeff = { 2.707768649521343f, 0.390393176362524f, -2.018231845635338f, 0.697604163191230f }, // d_phi
|
||||
.k21_coeff = { 4.135329288854244e+02f, -3.173913574866715e+02f, 50.321199991176265f, 10.217217753280829f }, // theta
|
||||
.k22_coeff = { 48.042446261620519f, -45.292268634116219f, 13.273044296221686f, 0.006982339078710f }, // d_theta
|
||||
.k23_coeff = { 14.015246608117772f, 10.813301135732024f, -18.216050987625373f, 6.078912501009629f }, // x
|
||||
.k24_coeff = { 21.698411755946285f, 5.621435092936538f, -18.016608013978576f, 6.611542756343175f }, // d_x
|
||||
.k25_coeff = { 4.798120071828828e+02f, -4.728222224549831e+02f, 1.591181202824602e+02f, -6.421997865768036f }, // phi
|
||||
.k26_coeff = { 59.794709871063546f, -60.418062715734166f, 20.991263455753383f, -1.388418937916963f }, // d_phi
|
||||
|
||||
// .k11_coeff = { -1.996305368054721e+02f, 2.260001266392926e+02f, -1.009632659573521e+02f, 2.651403223110949e+00f }, // theta
|
||||
// .k12_coeff = { 1.961704292162323e+00f, 8.251913348469651e-01f, -6.073749575127879e+00f, 3.535645826465822e-01f }, // d_theta
|
||||
// .k13_coeff = { -8.161081261310467e+01f, 8.274264960693915e+01f, -2.904800049859091e+01f, 1.653742354439720e+00f }, // x
|
||||
// .k14_coeff = { -7.455421501651551e+01f, 7.622235638964226e+01f, -2.773307975245958e+01f, 1.535810571728176e+00f }, // d_x
|
||||
// .k15_coeff = { -9.027722377713712e+00f, 2.891936105315331e+01f, -2.106785573613789e+01f, 4.948383450314285e+00f }, // phi
|
||||
// .k16_coeff = { 3.049839709048039e+00f, 4.627952499276505e-02f, -1.922806522134511e+00f, 6.902507101472589e-01f }, // d_phi
|
||||
// .k21_coeff = { 4.373445515700652e+02f, -3.391497363529634e+02f, 5.569210421652366e+01f, 1.081229141610274e+01f }, // theta
|
||||
// .k22_coeff = { 5.284103137531328e+01f, -5.028796043573002e+01f, 1.502532265509508e+01f, -3.077550945526411e-02f }, // d_theta
|
||||
// .k23_coeff = { 3.111787495894651e+01f, 4.872956811853720e+00f, -2.278008499711769e+01f, 8.426190283284837e+00f }, // x
|
||||
// .k24_coeff = { 3.329235562185018e+01f, -5.071568882184936e-01f, -1.928141689016582e+01f, 7.776043600153916e+00f }, // d_x
|
||||
// .k25_coeff = { 4.604866240014122e+02f, -4.577537299814802e+02f, 1.557742271762380e+02f, -6.272551369660446e+00f }, // phi
|
||||
// .k26_coeff = { 5.608646259671771e+01f, -5.728239809509379e+01f, 2.016452460533993e+01f, -1.320817827839268e+00f }, // d_phi
|
||||
|
||||
.lqr_gains = {
|
||||
.k11_coeff = { -1.518517857889991e+02f, 1.947427875897828e+02f, -1.148120822481098e+02f, 2.924883559935997e+00f }, // theta
|
||||
.k12_coeff = { 1.210535496345407e+01f, -9.650112118530574e+00f, -7.795614823592060e+00f, 4.657327020761534e-01f }, // d_theta
|
||||
.k13_coeff = { -1.165837444184349e+02f, 1.210192009641869e+02f, -4.426716583574083e+01f, 2.303172586380976e+00f }, // x
|
||||
.k14_coeff = { -1.286052871916038e+02f, 1.353354867883034e+02f, -5.203199795488686e+01f, 2.622579832615006e+00f }, // d_x
|
||||
.k15_coeff = { -1.105374347374162e+01f, 3.229730763109554e+01f, -2.423106279412239e+01f, 6.195204290845447e+00f }, // phi
|
||||
.k16_coeff = { 8.260175276799853e-01f, 2.318023492852569e+00f, -2.927935219327240e+00f, 8.556202879801391e-01f }, // d_phi
|
||||
.k21_coeff = { 5.970052798954075e+02f, -5.263867571125131e+02f, 1.275655583254851e+02f, 1.140189988320814e+01f }, // theta
|
||||
.k22_coeff = { 7.451585479025395e+01f, -7.785445709151199e+01f, 2.827386050355351e+01f, -1.578620610065337e-01f }, // d_theta
|
||||
.k23_coeff = { 4.622306333044981e+01f, 4.325117706159210e+00f, -3.407203207833582e+01f, 1.455475256803718e+01f }, // x
|
||||
.k24_coeff = { 6.870578295004115e+01f, -1.301773374139566e+01f, -3.211146583952794e+01f, 1.644195610244500e+01f }, // d_x
|
||||
.k25_coeff = { 4.493994591642744e+02f, -4.563158073471503e+02f, 1.605396531175411e+02f, -3.691208683727941e+00f }, // phi
|
||||
.k26_coeff = { 5.771012099214764e+01f, -5.915171391239870e+01f, 2.096920183531924e+01f, -1.205283154896878e+00f }, // d_phi
|
||||
},
|
||||
.lqr_param.max_joint_torque = 20.0f, // 关节电机最大力矩 20Nm
|
||||
.lqr_param.max_wheel_torque = 4.5f, // 轮毂电机最大力矩 2.5Nm
|
||||
|
||||
@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
|
||||
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
|
||||
B=double(B);
|
||||
|
||||
Q=diag([700 1 5000 100 3000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
|
||||
R=[240 0;0 25]; %T Tp
|
||||
Q=diag([700 1 2000 1500 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
|
||||
R=[140 0;0 15]; %T Tp
|
||||
|
||||
K=lqr(A,B,Q,R);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user