\u672c\u6587\u9762\u5411\u7b2c\u4e00\u6b21\u63a5\u89e6 ADRC \u7684\u8bfb\u8005，\u4ece\u95ed\u73af\u63a7\u5236\u548c PID \u5f00\u59cb，\u9010\u6b65\u63a8\u5bfc\u4e00\u9636\u3001\u4e8c\u9636 LADRC，\u89e3\u91ca TD\u3001ESO\u3001\u8bef\u5dee\u53cd\u9988\u548c\u6270\u52a8\u8865\u507f，\u5e76\u7ed9\u51fa\u9002\u5408 STM32 \u7684 C \u8bed\u8a00\u5b9e\u73b0\u3001\u53c2\u6570\u6574\u5b9a\u987a\u5e8f\u4ee5\u53ca\u7535\u673a\u4e0a\u673a\u6ce8\u610f\u4e8b\u9879\u3002<\/p>\n
ADRC（Active Disturbance Rejection Control，\u81ea\u6297\u6270\u63a7\u5236）\u7684\u6838\u5fc3\u4e0d\u662f\u201c\u6362\u4e00\u79cd PID\u201d，\u800c\u662f\u628a\u672a\u77e5\u8d1f\u8f7d\u3001\u6469\u64e6\u3001\u6a21\u578b\u8bef\u5dee\u548c\u5916\u90e8\u6270\u52a8\u7edf\u4e00\u770b\u6210\u603b\u6270\u52a8，\u5229\u7528\u6269\u5f20\u72b6\u6001\u89c2\u6d4b\u5668（ESO）\u5b9e\u65f6\u4f30\u8ba1，\u518d\u5728\u63a7\u5236\u8f93\u51fa\u4e2d\u4e3b\u52a8\u8865\u507f\u3002<\/p>\n
\u5982\u679c\u53ea\u8bb0\u4f4f\u590d\u6742\u7684\u00a0fhan()\u00a0\u6216\u51e0\u7ec4\u53c2\u6570，\u800c\u6ca1\u6709\u7406\u89e3\u5bf9\u8c61\u9636\u6570\u3001b0\u3001\u91c7\u6837\u5468\u671f\u548c ESO \u72b6\u6001\u542b\u4e49，\u90a3\u4e48 ADRC \u5f88\u5bb9\u6613\u53d8\u6210\u4e00\u6bb5\u201c\u80fd\u7f16\u8bd1\u4f46\u4e0d\u4f1a\u8c03\u201d\u7684\u4ee3\u7801\u3002\u672c\u6587\u5e0c\u671b\u89e3\u51b3\u7684\u6b63\u662f\u8fd9\u4e2a\u95ee\u9898：\u4e0d\u4ec5\u7ed9\u51fa\u516c\u5f0f，\u8fd8\u8981\u8bf4\u660e\u516c\u5f0f\u600e\u6837\u843d\u5230\u4ee3\u7801\u3001\u53c2\u6570\u9519\u4e86\u4f1a\u51fa\u73b0\u4ec0\u4e48\u73b0\u8c61，\u4ee5\u53ca\u600e\u6837\u4ece\u96f6\u590d\u73b0\u4e00\u4e2a\u6700\u5c0f\u53ef\u7528\u7684 LADRC\u3002<\/p>\n
\u5173\u952e\u8bcd：\u00a0ADRC\u3001LADRC\u3001TD\u3001ESO\u3001STM32\u3001\u7535\u673a\u63a7\u5236\u3001PID\u3001\u6270\u52a8\u89c2\u6d4b\u5668<\/p>\n
\u4ee5\u7535\u673a\u4f4d\u7f6e\u63a7\u5236\u4e3a\u4f8b，\u6700\u57fa\u672c\u7684\u95ed\u73af\u662f：<\/p>\n
\u76ee\u6807\u4f4d\u7f6e set
\n |
\n v
\n\u63a7\u5236\u5668 PID\/ADRC —> \u7535\u6d41\u6216\u8f6c\u77e9\u6307\u4ee4 u —> \u9a71\u52a8\u5668\u548c\u7535\u673a —> \u5b9e\u9645\u4f4d\u7f6e y
\n ^ |
\n |_______________________________________________|
\n \u7f16\u7801\u5668\u53cd\u9988<\/p>\n
\u63a7\u5236\u5668\u6bcf\u9694\u56fa\u5b9a\u65f6\u95f4\u6267\u884c\u4e00\u6b21：\u8bfb\u53d6\u76ee\u6807\u548c\u53cd\u9988，\u8ba1\u7b97\u8f93\u51fa，\u518d\u628a\u8f93\u51fa\u53d1\u9001\u7ed9\u7535\u673a\u9a71\u52a8\u5668\u3002<\/p>\n
\u5047\u8bbe\u76ee\u6807\u4f4d\u7f6e\u662f\u00a01000，\u7f16\u7801\u5668\u53cd\u9988\u662f\u00a0800，\u90a3\u4e48\u4f4d\u7f6e\u8bef\u5dee\u4e3a：<\/p>\n
e = set – feedback = 1000 – 800 = 200<\/p>\n
PID \u548c ADRC \u90fd\u4f1a\u5229\u7528\u8fd9\u4e2a\u8bef\u5dee\u4ea7\u751f\u63a7\u5236\u8f93\u51fa，\u4f46\u4e8c\u8005\u5904\u7406\u672a\u77e5\u8d1f\u8f7d\u7684\u65b9\u5f0f\u4e0d\u540c\u3002<\/p>\n
PID \u7684\u57fa\u672c\u5f62\u5f0f\u662f：<\/p>\n
u=Kpe+Ki\u222bedt+Kddedtu=Kp\u200be+Ki\u200b\u222bedt+Kd\u200bdtde\u200b<\/p>\n
\u5f53\u8d1f\u8f7d\u7a81\u7136\u589e\u52a0\u65f6，PID \u7684\u5178\u578b\u8fc7\u7a0b\u662f：<\/p>\n
\u8d1f\u8f7d\u589e\u52a0 -> \u7535\u673a\u504f\u79bb\u76ee\u6807 -> \u8bef\u5dee\u589e\u5927 -> PID\u589e\u52a0\u8f93\u51fa<\/p>\n
\u4e5f\u5c31\u662f\u8bf4，PID \u901a\u5e38\u8981\u7b49\u8bef\u5dee\u51fa\u73b0\u540e，\u518d\u901a\u8fc7\u6bd4\u4f8b\u548c\u79ef\u5206\u8fdb\u884c\u8865\u6551\u3002<\/p>\n
ADRC \u7684\u601d\u8def\u66f4\u76f4\u63a5：\u9664\u4e86\u7ea0\u6b63\u8ddf\u8e2a\u8bef\u5dee，\u8fd8\u5efa\u7acb\u4e00\u4e2a\u89c2\u6d4b\u5668，\u4f30\u8ba1\u201c\u7cfb\u7edf\u4e2d\u6ca1\u6709\u88ab\u6a21\u578b\u89e3\u91ca\u7684\u90a3\u90e8\u5206\u4f5c\u7528\u201d，\u518d\u4e3b\u52a8\u8865\u507f\u5b83\u3002<\/p>\n
\u76ee\u6807\u503c
\n |
\n v
\nTD \u8ddf\u8e2a\u5fae\u5206\u5668
\n |
\n v
\n\u8bef\u5dee\u53cd\u9988\u63a7\u5236 ——+
\n |
\nESO \u72b6\u6001\u89c2\u6d4b\u5668 —>+– \u6270\u52a8\u8865\u507f –> \u8f93\u51fa u
\n ^
\n |
\n\u7535\u673a\u53cd\u9988<\/p>\n
\u56e0\u6b64\u53ef\u4ee5\u5148\u8bb0\u4f4f\u4e00\u53e5\u8bdd：<\/p>\n
ADRC = \u76ee\u6807\u8f68\u8ff9\u5b89\u6392 + \u72b6\u6001\/\u6270\u52a8\u89c2\u6d4b + \u8bef\u5dee\u53cd\u9988 + \u603b\u6270\u52a8\u8865\u507f\u3002<\/p>\n
\u4e0d\u8fc7，ADRC \u4e0d\u662f\u201c\u81ea\u52a8\u8c03\u53c2\u63a7\u5236\u5668\u201d\u3002b0\u3001\u63a7\u5236\u5468\u671f\u3001\u63a7\u5236\u5668\u5e26\u5bbd\u548c\u89c2\u6d4b\u5668\u5e26\u5bbd\u8bbe\u7f6e\u9519\u8bef\u65f6，\u5b83\u540c\u6837\u4f1a\u632f\u8361\u3001\u9971\u548c\u751a\u81f3\u53cd\u5411\u5931\u63a7\u3002<\/p>\n
\u8fd9\u662f\u4f7f\u7528 ADRC \u524d\u6700\u5bb9\u6613\u88ab\u5ffd\u7565\u7684\u4e00\u6b65\u3002<\/p>\n
\u5982\u679c\u63a7\u5236\u8f93\u5165\u662f\u7535\u6d41\/\u8f6c\u77e9\u6307\u4ee4，\u88ab\u63a7\u91cf\u662f\u7535\u673a\u901f\u5ea6，\u53ef\u4ee5\u628a\u4f4e\u9891\u52a8\u6001\u8fd1\u4f3c\u4e3a：<\/p>\n
y\u02d9=f+b0uy\u02d9\u200b=f+b0\u200bu<\/p>\n
\u5176\u4e2d：<\/p>\n
\u6b64\u65f6\u53ef\u4f7f\u7528\u4e8c\u9636 ESO：<\/p>\n
z1 \u2248 \u901f\u5ea6 y
\nz2 \u2248 \u603b\u6270\u52a8 f<\/p>\n
\u5982\u679c\u88ab\u63a7\u91cf\u662f\u4f4d\u7f6e，\u5219\u5e38\u5199\u6210：<\/p>\n
y\u00a8=f+b0uy\u00a8\u200b=f+b0\u200bu<\/p>\n
\u4ee4：<\/p>\n
x1=y,x2=y\u02d9,x3=fx1\u200b=y,x2\u200b=y\u02d9\u200b,x3\u200b=f<\/p>\n
\u5f97\u5230：<\/p>\n
x\u02d91=x2x\u02d92=x3+b0ux\u02d93\u22480x\u02d91\u200bx\u02d92\u200bx\u02d93\u200b\u200b=x2\u200b=x3\u200b+b0\u200bu\u22480\u200b<\/p>\n
\u6b64\u65f6\u4f7f\u7528\u4e09\u9636 ESO：<\/p>\n
z1 \u2248 \u4f4d\u7f6e
\nz2 \u2248 \u901f\u5ea6
\nz3 \u2248 \u603b\u6270\u52a8<\/p>\n
\u8fd9\u4e24\u4e2a\u7ed3\u6784\u4e0d\u80fd\u4ec5\u9760\u4fee\u6539\u53d8\u91cf\u540d\u4e92\u6362\u3002\u901f\u5ea6\u73af\u4e2d\u00a0z2\u00a0\u53ef\u4ee5\u662f\u603b\u6270\u52a8，\u800c\u4e8c\u9636\u4f4d\u7f6e\u73af\u4e2d\u00a0z2\u00a0\u662f\u901f\u5ea6\u3001z3\u00a0\u624d\u662f\u603b\u6270\u52a8\u3002\u9605\u8bfb\u4efb\u4f55 ADRC \u4ee3\u7801\u65f6，\u90fd\u5e94\u5148\u4ece ESO \u65b9\u7a0b\u5224\u65ad\u5b83\u5b9e\u73b0\u7684\u662f\u54ea\u4e00\u79cd\u5bf9\u8c61\u6a21\u578b\u3002<\/p>\n
\u76ee\u6807\u4f4d\u7f6e\u4ece\u00a00\u00a0\u7a81\u7136\u8df3\u5230\u00a01000\u00a0\u65f6，\u76f4\u63a5\u9001\u5165\u63a7\u5236\u5668\u53ef\u80fd\u4ea7\u751f\u5f88\u5927\u7684\u77ac\u65f6\u8f93\u51fa\u3002TD（Tracking Differentiator，\u8ddf\u8e2a\u5fae\u5206\u5668）\u7528\u4e8e\u751f\u6210\u5e73\u6ed1\u76ee\u6807\u53ca\u5176\u53d8\u5316\u7387：<\/p>\n
\u539f\u59cb\u76ee\u6807：0 —————- 1000
\nTD\u4f4d\u7f6ev1：0 -> 1 -> 3 -> 6 -> … -> 1000
\nTD\u901f\u5ea6v2：0 -> \u589e\u5927 -> \u4fdd\u6301 -> \u51cf\u5c0f -> 0<\/p>\n
TD \u72b6\u6001\u66f4\u65b0\u53ef\u4ee5\u7406\u89e3\u4e3a\u57fa\u7840\u8fd0\u52a8\u5b66：<\/p>\n
v1 = v1 + h * v2;
\nv2 = v2 + h * fh;<\/p>\n
\u5176\u4e2d：<\/p>\n
\u9879\u76ee\u4e2d\u7684\u79bb\u6563\u00a0fhan()\u00a0\u6838\u5fc3\u4ee3\u7801\u5982\u4e0b：<\/p>\n
static float adrc_fhan(float position_error,
\n float velocity,
\n float r,
\n float h)
\n{
\n float d = r * h;
\n float d0 = h * d;
\n float y = position_error + h * velocity;
\n float a1 = sqrtf(d * d + 8.0f * r * fabsf(y));
\n float a;<\/p>\n
if (fabsf(y) <= d0)
\n {
\n a = velocity + y \/ h;
\n }
\n else if (y > 0.0f)
\n {
\n a = velocity + (a1 – d) \/ 2.0f;
\n }
\n else
\n {
\n a = velocity – (a1 – d) \/ 2.0f;
\n }<\/p>\n
if (a > d) a = d;
\n if (a < -d) a = -d;<\/p>\n
return -r * a \/ d;
\n}<\/p>\n
\u5b83\u7684\u76f4\u89c2\u76ee\u6807\u662f：\u79bb\u76ee\u6807\u8f83\u8fdc\u65f6\u52a0\u901f\u8ffd\u8d76，\u63a5\u8fd1\u76ee\u6807\u65f6\u6839\u636e\u5f53\u524d\u4f4d\u7f6e\u548c\u5f53\u524d\u901f\u5ea6\u63d0\u524d\u5236\u52a8\u3002<\/p>\n
\u9700\u8981\u6ce8\u610f：h\u00a0\u548c\u00a0r\u00a0\u5fc5\u987b\u5927\u4e8e\u96f6，\u5426\u5219\u4ee3\u7801\u53ef\u80fd\u51fa\u73b0\u9664\u96f6\u6216\u65e0\u6548\u5e73\u65b9\u6839\u3002TD \u4e0d\u662f ADRC \u552f\u4e00\u53ef\u7528\u7684\u76ee\u6807\u89c4\u5212\u65b9\u5f0f，\u5de5\u7a0b\u4e2d\u4e5f\u53ef\u4ee5\u5148\u7528\u659c\u5761\u3001\u68af\u5f62\u901f\u5ea6\u89c4\u5212\u6216 S \u66f2\u7ebf\u66ff\u4ee3，\u4ee5\u964d\u4f4e\u8c03\u8bd5\u590d\u6742\u5ea6\u3002<\/p>\n
\u4ee5\u4e8c\u9636\u4f4d\u7f6e\u6a21\u578b\u4e3a\u4f8b：<\/p>\n
y\u00a8=f+b0uy\u00a8\u200b=f+b0\u200bu<\/p>\n
\u7ebf\u6027\u6269\u5f20\u72b6\u6001\u89c2\u6d4b\u5668\u53ef\u4ee5\u5199\u6210：<\/p>\n
e=z1\u2212yz\u02d91=z2\u2212\u03b21ez\u02d92=z3\u2212\u03b22e+b0uz\u02d93=\u2212\u03b23eez\u02d91\u200bz\u02d92\u200bz\u02d93\u200b\u200b=z1\u200b\u2212y=z2\u200b\u2212\u03b21\u200be=z3\u200b\u2212\u03b22\u200be+b0\u200bu=\u2212\u03b23\u200be\u200b<\/p>\n
\u4f7f\u7528\u663e\u5f0f\u6b27\u62c9\u79bb\u6563\u540e：<\/p>\n
e = z1 – feedback;<\/p>\n
z1_next = z1 + h * (z2 – beta1 * e);
\nz2_next = z2 + h * (z3 – beta2 * e + b0 * u_last);
\nz3_next = z3 + h * (-beta3 * e);<\/p>\n
\u8fd9\u91cc\u7684\u00a0u_last\u00a0\u662f\u4e0a\u4e00\u5468\u671f\u5b9e\u9645\u53d1\u9001\u7684\u3001\u7ecf\u8fc7\u9650\u5e45\u7684\u63a7\u5236\u6307\u4ee4\u3002\u5f53\u524d\u53cd\u9988\u662f\u4e0a\u4e00\u5468\u671f\u8f93\u51fa\u4f5c\u7528\u540e\u7684\u7ed3\u679c，\u56e0\u6b64\u89c2\u6d4b\u5668\u5e94\u4f7f\u7528\u4e0a\u4e00\u5468\u671f\u8f93\u51fa\u89e3\u91ca\u5f53\u524d\u6d4b\u91cf\u3002<\/p>\n
ESO \u6bcf\u5468\u671f\u90fd\u505a\u4e24\u4ef6\u4e8b：<\/p>\n
\u4f8b\u5982\u5916\u90e8\u8d1f\u8f7d\u7a81\u7136\u589e\u52a0，\u7535\u673a\u5b9e\u9645\u52a0\u901f\u5ea6\u5c0f\u4e8e\u6a21\u578b\u9884\u6d4b\u503c，\u4f4d\u7f6e\u9884\u6d4b\u4f1a\u9010\u6e10\u504f\u79bb\u7f16\u7801\u5668\u53cd\u9988\u3002ESO \u5c06\u8fd9\u90e8\u5206\u65e0\u6cd5\u7531\u00a0b0*u\u00a0\u89e3\u91ca\u7684\u5dee\u5f02\u63a8\u5165\u00a0z3，\u6700\u7ec8\u628a\u5b83\u770b\u4f5c\u603b\u6270\u52a8\u3002<\/p>\n
\u4ee4 ESO \u4e09\u4e2a\u6781\u70b9\u90fd\u4f4d\u4e8e\u00a0\u2212\u03c90\u2212\u03c90\u200b，\u6709：<\/p>\n
(s+\u03c90)3=s3+3\u03c90s2+3\u03c902s+\u03c903(s+\u03c90\u200b)3=s3+3\u03c90\u200bs2+3\u03c902\u200bs+\u03c903\u200b<\/p>\n
\u56e0\u6b64：<\/p>\n
beta1 = 3.0f * w0;
\nbeta2 = 3.0f * w0 * w0;
\nbeta3 = w0 * w0 * w0;<\/p>\n
w0\u00a0\u8d8a\u5927，\u89c2\u6d4b\u8d8a\u5feb，\u4f46\u7f16\u7801\u5668\u91cf\u5316\u566a\u58f0\u3001\u901a\u4fe1\u6296\u52a8\u548c\u673a\u68b0\u632f\u52a8\u4e5f\u8d8a\u5bb9\u6613\u8fdb\u5165\u00a0z2\u3001z3\u3002ESO \u5e26\u5bbd\u4e0d\u662f\u8d8a\u5927\u8d8a\u597d\u3002<\/p>\n
\u4e8c\u9636\u4f4d\u7f6e LADRC \u4f7f\u7528：<\/p>\n
e1=v1\u2212z1,e2=v2\u2212z2e1\u200b=v1\u200b\u2212z1\u200b,e2\u200b=v2\u200b\u2212z2\u200b<\/p>\n
\u57fa\u7840\u63a7\u5236\u4f5c\u7528\u4e3a：<\/p>\n
u0=Kpe1+Kde2u0\u200b=Kp\u200be1\u200b+Kd\u200be2\u200b<\/p>\n
\u9009\u62e9\u63a7\u5236\u5668\u5e26\u5bbd\u00a0\u03c9c\u03c9c\u200b：<\/p>\n
Kp=\u03c9c2,Kd=2\u03c9cKp\u200b=\u03c9c2\u200b,Kd\u200b=2\u03c9c\u200b<\/p>\n
\u5bf9\u5e94\u671f\u671b\u95ed\u73af\u7279\u5f81\u591a\u9879\u5f0f：<\/p>\n
(s+\u03c9c)2=s2+2\u03c9cs+\u03c9c2(s+\u03c9c\u200b)2=s2+2\u03c9c\u200bs+\u03c9c2\u200b<\/p>\n
\u5bf9\u8c61\u6a21\u578b\u662f：<\/p>\n
y\u00a8=f+b0uy\u00a8\u200b=f+b0\u200bu<\/p>\n
\u5e0c\u671b\u8865\u507f\u540e：<\/p>\n
y\u00a8\u2248u0y\u00a8\u200b\u2248u0\u200b<\/p>\n
ESO \u7ed9\u51fa\u00a0z3\u2248fz3\u200b\u2248f，\u4e8e\u662f：<\/p>\n
u0=z3+b0uu0\u200b=z3\u200b+b0\u200bu<\/p>\n
\u79fb\u9879\u5f97\u5230：<\/p>\n
u=u0\u2212z3b0u=b0\u200bu0\u200b\u2212z3\u200b\u200b\u200b<\/p>\n
\u5bf9\u5e94\u4ee3\u7801：<\/p>\n
float u0 = kp * e1 + kd * e2;
\nfloat u = (u0 – z3) \/ b0;<\/p>\n
\u4e0b\u9762\u4e24\u79cd\u5199\u6cd5\u5e76\u4e0d\u7b49\u4ef7：<\/p>\n
u = (u0 – z3) \/ b0; \/* \u6807\u51c6\u5f62\u5f0f *\/
\nu = u0 – z3 \/ b0; \/* \u53ea\u8ba9\u6270\u52a8\u9879\u9664\u4ee5b0 *\/<\/p>\n
\u5047\u8bbe：<\/p>\n
u0 = 40
\nz3 = -6
\nb0 = 0.5<\/p>\n
\u6807\u51c6\u5f62\u5f0f\u5f97\u5230：<\/p>\n
u = (40-(-6))\/0.5 = 92<\/p>\n
\u4ee3\u56de\u5bf9\u8c61：<\/p>\n
y'' = -6 + 0.5\u00d792 = 40<\/p>\n
\u800c\u7b2c\u4e8c\u79cd\u5199\u6cd5\u5f97\u5230：<\/p>\n
u = 40-(-6\/0.5) = 52
\ny'' = -6 + 0.5\u00d752 = 20<\/p>\n
\u8fd9\u4f1a\u6539\u53d8\u63a7\u5236\u5668\u5b9e\u9645\u5e26\u5bbd\u3002\u5ba1\u67e5\u9879\u76ee\u6e90\u7801\u65f6，\u5e94\u7279\u522b\u68c0\u67e5\u62ec\u53f7\u548c\u00a0b0\u00a0\u7684\u5b9a\u4e49，\u800c\u4e0d\u662f\u4ec5\u51ed\u53d8\u91cf\u540d\u5224\u65ad\u5b9e\u73b0\u6b63\u786e\u3002<\/p>\n
\u4e3a\u4e86\u5148\u9a8c\u8bc1 ESO \u548c\u6270\u52a8\u8865\u507f，\u4e0b\u9762\u7684\u6700\u5c0f\u5b9e\u73b0\u6682\u65f6\u4e0d\u5305\u542b TD \u548c\u5faa\u73af\u89d2\u5ea6\u5904\u7406\u3002\u76ee\u6807\u4f4d\u7f6e\u4e0e\u76ee\u6807\u901f\u5ea6\u7531\u5916\u90e8\u76f4\u63a5\u63d0\u4f9b\u3002<\/p>\n
#ifndef LADRC2_H
\n#define LADRC2_H<\/p>\n
#include <stdbool.h><\/p>\n
typedef struct
\n{
\n float h;
\n float b0;
\n float out_max;<\/p>\n
float beta1;
\n float beta2;
\n float beta3;<\/p>\n
float kp;
\n float kd;<\/p>\n
float z1;
\n float z2;
\n float z3;<\/p>\n
float out;
\n} ladrc2_t;<\/p>\n
bool LADRC2_Init(ladrc2_t *ctrl,
\n float h,
\n float b0,
\n float w0,
\n float wc,
\n float out_max,
\n float initial_feedback);<\/p>\n
float LADRC2_Update(ladrc2_t *ctrl,
\n float feedback,
\n float target_position,
\n float target_velocity);<\/p>\n
#endif<\/p>\n
#include "ladrc2.h"
\n#include <math.h>
\n#include <stddef.h><\/p>\n
#define LADRC_B0_MIN 1e-4f<\/p>\n
static float ladrc_clamp(float value, float limit)
\n{
\n if (value > limit) return limit;
\n if (value < -limit) return -limit;
\n return value;
\n}<\/p>\n
bool LADRC2_Init(ladrc2_t *ctrl,
\n float h,
\n float b0,
\n float w0,
\n float wc,
\n float out_max,
\n float initial_feedback)
\n{
\n if (ctrl == NULL ||
\n h <= 0.0f ||
\n w0 <= 0.0f ||
\n wc <= 0.0f ||
\n out_max <= 0.0f ||
\n fabsf(b0) < LADRC_B0_MIN)
\n {
\n return false;
\n }<\/p>\n
ctrl->h = h;
\n ctrl->b0 = b0;
\n ctrl->out_max = out_max;<\/p>\n
ctrl->beta1 = 3.0f * w0;
\n ctrl->beta2 = 3.0f * w0 * w0;
\n ctrl->beta3 = w0 * w0 * w0;<\/p>\n
ctrl->kp = wc * wc;
\n ctrl->kd = 2.0f * wc;<\/p>\n
\/* \u4e0a\u7535\u65f6\u5bf9\u9f50\u5f53\u524d\u4f4d\u7f6e，\u907f\u514d\u975e\u96f6\u4f4d\u7f6e\u4ea7\u751f\u5de8\u5927\u89c2\u6d4b\u8bef\u5dee *\/
\n ctrl->z1 = initial_feedback;
\n ctrl->z2 = 0.0f;
\n ctrl->z3 = 0.0f;
\n ctrl->out = 0.0f;<\/p>\n
return true;
\n}<\/p>\n
float LADRC2_Update(ladrc2_t *ctrl,
\n float feedback,
\n float target_position,
\n float target_velocity)
\n{
\n float e;
\n float z1_next;
\n float z2_next;
\n float z3_next;
\n float e1;
\n float e2;
\n float u0;
\n float u;<\/p>\n
if (ctrl == NULL)
\n return 0.0f;<\/p>\n
\/* ESO：\u4f7f\u7528\u5f53\u524d\u53cd\u9988\u548c\u4e0a\u4e00\u5468\u671f\u9650\u5e45\u540e\u7684\u5b9e\u9645\u6307\u4ee4 *\/
\n e = ctrl->z1 – feedback;<\/p>\n
z1_next = ctrl->z1
\n + ctrl->h * (ctrl->z2 – ctrl->beta1 * e);<\/p>\n
z2_next = ctrl->z2
\n + ctrl->h * (ctrl->z3
\n – ctrl->beta2 * e
\n + ctrl->b0 * ctrl->out);<\/p>\n
z3_next = ctrl->z3
\n + ctrl->h * (-ctrl->beta3 * e);<\/p>\n
ctrl->z1 = z1_next;
\n ctrl->z2 = z2_next;
\n ctrl->z3 = z3_next;<\/p>\n
\/* \u4f4d\u7f6e\u548c\u901f\u5ea6\u8bef\u5dee\u53cd\u9988 *\/
\n e1 = target_position – ctrl->z1;
\n e2 = target_velocity – ctrl->z2;
\n u0 = ctrl->kp * e1 + ctrl->kd * e2;<\/p>\n
\/* \u603b\u6270\u52a8\u8865\u507f *\/
\n u = (u0 – ctrl->z3) \/ ctrl->b0;<\/p>\n
\/* \u4fdd\u5b58\u771f\u5b9e\u53d1\u9001\u503c，\u4e0b\u4e00\u5468\u671fESO\u7ee7\u7eed\u4f7f\u7528 *\/
\n ctrl->out = ladrc_clamp(u, ctrl->out_max);
\n return ctrl->out;
\n}<\/p>\n
static ladrc2_t motor_position_adrc;<\/p>\n
void MotorControl_Init(float current_position)
\n{
\n bool ok = LADRC2_Init(&motor_position_adrc,
\n 0.001f, \/* \u771f\u5b9e\u63a7\u5236\u5468\u671f：1 ms *\/
\n 0.5f, \/* b0：\u9700\u8981\u5b9e\u9a8c\u8fa8\u8bc6 *\/
\n 15.0f, \/* ESO\u5e26\u5bbd *\/
\n 5.0f, \/* \u63a7\u5236\u5668\u5e26\u5bbd *\/
\n 500.0f, \/* \u8c03\u8bd5\u9636\u6bb5\u4fdd\u5b88\u9650\u5e45 *\/
\n current_position);<\/p>\n
if (!ok)
\n {
\n \/* \u53c2\u6570\u65e0\u6548：\u7981\u6b62\u4f7f\u80fd\u7535\u673a *\/
\n }
\n}<\/p>\n
void MotorControl_1ms(float position_feedback,
\n float position_target)
\n{
\n float output = LADRC2_Update(&motor_position_adrc,
\n position_feedback,
\n position_target,
\n 0.0f);<\/p>\n
Motor_SendCurrentCommand(output);
\n}<\/p>\n
\u8fd9\u6bb5\u4ee3\u7801\u662f\u5b66\u4e60\u548c\u4eff\u771f\u7684\u6700\u5c0f\u6838\u5fc3，\u4e0d\u7b49\u4e8e\u53ef\u76f4\u63a5\u4e0a\u771f\u5b9e\u673a\u5668\u4eba\u3002\u5b9e\u9645\u5de5\u7a0b\u8fd8\u9700\u8981\u89d2\u5ea6\u5c55\u5f00\u3001\u6545\u969c\u72b6\u6001\u673a\u3001\u901a\u4fe1\u8d85\u65f6\u3001\u8f6f\u786c\u9650\u4f4d\u3001\u6025\u505c\u548c\u7535\u6d41\u4fdd\u62a4\u3002<\/p>\n
\u52a0\u5165 TD \u548c\u5faa\u73af\u89d2\u5ea6\u5904\u7406\u540e，\u4e00\u4e2a\u5468\u671f\u901a\u5e38\u6309\u4ee5\u4e0b\u987a\u5e8f\u8fd0\u884c：<\/p>\n
1. \u4fdd\u5b58\u76ee\u6807\u548c\u53cd\u9988
\n2. \u5bf9\u5faa\u73af\u89d2\u5ea6\u505a\u6700\u77ed\u8def\u5f84\u5c55\u5f00
\n3. TD\u66f4\u65b0v1\u3001v2
\n4. ESO\u4f7f\u7528\u53cd\u9988\u548c\u4e0a\u4e00\u5468\u671f\u8f93\u51fa\u66f4\u65b0z1\u3001z2\u3001z3
\n5. \u8ba1\u7b97e1\u3001e2\u548cu0
\n6. \u4f7f\u7528z3\u8fdb\u884c\u6270\u52a8\u8865\u507f
\n7. \u5bf9\u6700\u7ec8\u8f93\u51fa\u9650\u5e45
\n8. \u4fdd\u5b58\u5b9e\u9645\u8f93\u51fa，\u4f9b\u4e0b\u4e00\u5468\u671fESO\u4f7f\u7528<\/p>\n
\u5982\u679c\u7f16\u7801\u5668\u8303\u56f4\u4e3a\u00a00~8191，\u53cd\u9988\u4e3a\u00a08190\u3001\u76ee\u6807\u4e3a\u00a010，\u76f4\u63a5\u76f8\u51cf\u5f97\u5230\u00a0-8180，\u4f46\u6700\u77ed\u8def\u5f84\u5176\u5b9e\u662f\u6b63\u5411\u8de8\u96f6\u00a012\u00a0\u4e2a\u8ba1\u6570\u3002\u56e0\u6b64\u5faa\u73af\u91cf\u9700\u8981\u5c55\u5f00：<\/p>\n
float error = target – feedback;<\/p>\n
if (error >= 4096.0f)
\n error -= 8192.0f;
\nelse if (error < -4096.0f)
\n error += 8192.0f;<\/p>\n
\u901f\u5ea6\u4e0d\u662f\u5faa\u73af\u91cf，\u901f\u5ea6\u73af\u4e0d\u5e94\u4f7f\u7528\u8fd9\u79cd\u89d2\u5ea6\u8de8\u96f6\u903b\u8f91\u3002<\/p>\n
h\u00a0\u4e0d\u662f\u7528\u6765\u6539\u5584\u54cd\u5e94\u7684\u8c03\u53c2\u65cb\u94ae，\u800c\u662f\u7b97\u6cd5\u65f6\u95f4\u57fa\u51c6\u3002\u4efb\u52a1\u6bcf\u00a01 ms\u00a0\u8c03\u7528\u4e00\u6b21，\u5c31\u5e94\u8bbe\u7f6e：<\/p>\n
h = 0.001f;<\/p>\n
\u5b9e\u9645\u5468\u671f\u662f\u00a01 ms，\u4ee3\u7801\u5374\u586b\u5199\u00a00.01 s，TD \u548c ESO \u6bcf\u5468\u671f\u79ef\u5206\u91cf\u4f1a\u653e\u5927\u5341\u500d\u3002\u4e0a\u673a\u524d\u5fc5\u987b\u6d4b\u91cf\u5b9e\u9645\u5468\u671f\u548c\u6296\u52a8\u3002<\/p>\n
\u663e\u5f0f\u6b27\u62c9\u5b9e\u73b0\u53ef\u4ee5\u5148\u4fdd\u5b88\u68c0\u67e5：<\/p>\n
w0 \u00d7 h <= 0.1~0.2<\/p>\n
\u8fd9\u4e0d\u662f\u901a\u7528\u7a33\u5b9a\u6027\u8bc1\u660e，\u4f46\u9002\u5408\u4f5c\u4e3a\u521d\u59cb\u7b5b\u67e5\u3002<\/p>\n
\u4e8c\u9636\u4f4d\u7f6e\u6a21\u578b\u4e2d：<\/p>\n
b0\u2248\u0394\u52a0\u901f\u5ea6\u0394\u63a7\u5236\u6307\u4ee4b0\u200b\u2248\u0394\u63a7\u5236\u6307\u4ee4\u0394\u52a0\u901f\u5ea6\u200b<\/p>\n
\u4f8b\u5982\u63a7\u5236\u6307\u4ee4\u589e\u52a0\u00a01000，\u6d4b\u5f97\u52a0\u901f\u5ea6\u589e\u52a0\u7ea6\u00a0500 count\/s\u00b2：<\/p>\n
b0 \u2248 500\/1000 = 0.5<\/p>\n
\u9996\u5148\u8981\u786e\u8ba4\u7b26\u53f7\u3002\u6b63\u5411\u6307\u4ee4\u5e94\u4ea7\u751f\u6b63\u5411\u52a0\u901f\u5ea6，\u5426\u5219\u53cd\u9988\u65b9\u5411\u3001\u8f93\u51fa\u65b9\u5411\u6216\u00a0b0\u00a0\u7b26\u53f7\u81f3\u5c11\u6709\u4e00\u9879\u9519\u8bef\u3002\u65b9\u5411\u9519\u8bef\u65f6\u5fc5\u987b\u7acb\u5373\u505c\u673a，\u4e0d\u80fd\u901a\u8fc7\u589e\u5927\u5e26\u5bbd\u201c\u8c03\u56de\u6765\u201d\u3002<\/p>\n
wc\u589e\u5927 -> Kp\u3001Kd\u589e\u5927 -> \u54cd\u5e94\u66f4\u5feb -> \u8f93\u51fa\u9700\u6c42\u548c\u5ef6\u8fdf\u654f\u611f\u6027\u589e\u52a0<\/p>\n
\u4e8c\u9636\u4e34\u754c\u963b\u5c3c\u54cd\u5e94\u7684\u8c03\u8282\u65f6\u95f4\u53ef\u4ee5\u7c97\u7565\u4f30\u8ba1\u4e3a：<\/p>\n
\u8c03\u8282\u65f6\u95f4 \u2248 5\/wc<\/p>\n
\u5e0c\u671b\u7ea6\u00a01 s\u00a0\u7a33\u5b9a\u65f6，\u53ef\u4ee5\u4ece\u00a0wc\u22485 rad\/s\u00a0\u7684\u91cf\u7ea7\u5f00\u59cb，\u518d\u6839\u636e\u6267\u884c\u5668\u80fd\u529b\u8c03\u6574\u3002<\/p>\n
\u53ef\u4ee5\u5148\u4ece：<\/p>\n
w0 \u2248 3 \u00d7 wc<\/p>\n
\u5f00\u59cb\u3002w0\u00a0\u592a\u5c0f，z1\u3001z2\u00a0\u548c\u6270\u52a8\u4f30\u8ba1\u660e\u663e\u6ede\u540e；w0\u00a0\u592a\u5927，z2\u3001z3\u00a0\u4f1a\u653e\u5927\u7f16\u7801\u5668\u566a\u58f0，\u8f93\u51fa\u53ef\u80fd\u9ad8\u9891\u6296\u52a8\u3002<\/p>\n
\u5728\u672c\u6587\u4f7f\u7528\u7684\u00a0fhan()\u00a0\u5b9e\u73b0\u4e2d，fh\u00a0\u88ab\u9650\u5236\u5728\u00a0[-r,r]，\u56e0\u6b64\u00a0r\u00a0\u53ef\u4ee5\u8fd1\u4f3c\u7406\u89e3\u4e3a\u76ee\u6807\u8f68\u8ff9\u5141\u8bb8\u7684\u6700\u5927\u52a0\u901f\u5ea6\u3002<\/p>\n
r\u592a\u5c0f -> \u76ee\u6807\u8f68\u8ff9\u672c\u8eab\u5f88\u6162
\nr\u592a\u5927 -> \u63a5\u8fd1\u76f4\u63a5\u9636\u8dc3，\u8f93\u51fa\u5bb9\u6613\u9971\u548c\u548c\u51b2\u51fb<\/p>\n
\u8c03\u8bd5\u521d\u671f\u5fc5\u987b\u4fdd\u5b88\u8bbe\u7f6e\u3002\u5373\u4f7f\u9a71\u52a8\u5668\u5141\u8bb8\u00a0\u00b116384，\u4e5f\u4e0d\u4ee3\u8868\u7b2c\u4e00\u6b21\u4e0a\u7535\u5c31\u5e94\u8be5\u5f00\u653e\u5230\u6700\u5927\u8303\u56f4\u3002<\/p>\n
\u5f53\u8f93\u51fa\u957f\u671f\u9971\u548c\u65f6，\u7406\u8bba\u5173\u7cfb\u00a0y\u00a8\u2248u0y\u00a8\u200b\u2248u0\u200b\u00a0\u4e0d\u518d\u6210\u7acb\u3002\u6b64\u65f6\u7ee7\u7eed\u589e\u5927\u00a0wc\u00a0\u901a\u5e38\u53ea\u4f1a\u8ba9\u9971\u548c\u66f4\u4e25\u91cd\u3002<\/p>\n
\u4e0d\u8981\u540c\u65f6\u4fee\u6539\u6240\u6709\u53c2\u6570\u3002\u53ef\u9760\u7684\u987a\u5e8f\u662f：<\/p>\n
| \u4e00\u4f7f\u80fd\u5c31\u53cd\u5411\u52a0\u901f<\/td>\n | b0\u00a0\u7b26\u53f7\u3001\u53cd\u9988\u65b9\u5411\u3001\u8f93\u51fa\u65b9\u5411<\/td>\n<\/tr>\n | |||||||||||||||||||||||||||||||||||
| \u4e0a\u7535\u77ac\u95f4\u6253\u6ee1\u8f93\u51fa<\/td>\n | ESO\/TD \u521d\u59cb\u72b6\u6001\u672a\u5bf9\u9f50\u3001h\u00a0\u6216\u00a0b0\u00a0\u9519\u8bef<\/td>\n<\/tr>\n | |||||||||||||||||||||||||||||||||||
| \u6574\u4f53\u54cd\u5e94\u6162\u4e14\u6ca1\u6709\u9971\u548c<\/td>\n | wc\u00a0\u53ef\u80fd\u592a\u5c0f<\/td>\n<\/tr>\n | |||||||||||||||||||||||||||||||||||
| td_v1\u00a0\u672c\u8eab\u8ddf\u76ee\u6807\u5f88\u6162<\/td>\n | r\u00a0\u53ef\u80fd\u592a\u5c0f<\/td>\n<\/tr>\n | |||||||||||||||||||||||||||||||||||
| \u76ee\u6807\u9636\u8dc3\u65f6\u957f\u671f\u9971\u548c<\/td>\n | r\/wc\u00a0\u592a\u5927\u3001\u9650\u5e45\u8fc7\u5c0f\u6216\u00a0b0\u00a0\u4e0d\u51c6<\/td>\n<\/tr>\n | |||||||||||||||||||||||||||||||||||
| z1\u00a0\u660e\u663e\u843d\u540e\u7f16\u7801\u5668\u53cd\u9988<\/td>\n | w0\u00a0\u53ef\u80fd\u592a\u5c0f<\/td>\n<\/tr>\n | |||||||||||||||||||||||||||||||||||
| z2\/z3\u00a0\u9ad8\u9891\u6296\u52a8<\/td>\n | w0\u00a0\u592a\u5927\u3001\u7f16\u7801\u5668\u91cf\u5316\u6216\u673a\u68b0\u632f\u52a8\u660e\u663e<\/td>\n<\/tr>\n | |||||||||||||||||||||||||||||||||||
| \u52a0\u8f7d\u540e\u6062\u590d\u6162<\/td>\n | w0\u00a0\u592a\u5c0f\u3001b0\u00a0\u4e0d\u51c6\u6216\u8f93\u51fa\u80fd\u529b\u4e0d\u8db3<\/td>\n<\/tr>\n | |||||||||||||||||||||||||||||||||||
| \u7535\u673a\u9ad8\u9891\u9e23\u53eb<\/td>\n | \u4f18\u5148\u964d\u4f4e\u00a0w0，\u518d\u68c0\u67e5\u00a0wc\u00a0\u548c\u53cd\u9988\u566a\u58f0<\/td>\n<\/tr>\n | |||||||||||||||||||||||||||||||||||
| \u4f4e\u9891\u6765\u56de\u6446\u52a8<\/td>\n | wc\u00a0\u8fc7\u5927\u3001\u7cfb\u7edf\u5ef6\u8fdf\u6216\u673a\u68b0\u67d4\u6027<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n \n \u5341\u4e00\u3001\u7406\u60f3\u6a21\u578b\u4eff\u771f：ESO \u80fd\u5426\u771f\u7684\u4f30\u8ba1\u6270\u52a8<\/h3>\n\u4f7f\u7528\u865a\u62df\u4e8c\u9636\u7535\u673a：<\/p>\n x\u02d91=x2,x\u02d92=f+btrueux\u02d91\u200b=x2\u200b,x\u02d92\u200b=f+btrue\u200bu<\/p>\n \u53d6：<\/p>\n h = 0.001 s \u5728\u00a00.2 s\u00a0\u5c06\u4f4d\u7f6e\u76ee\u6807\u4ece\u00a00\u00a0\u6539\u4e3a\u00a01，\u5728\u00a02.0 s\u00a0\u7a81\u7136\u52a0\u5165\u6052\u5b9a\u6270\u52a8\u00a0f=-20，\u5f97\u5230\u90e8\u5206\u7ed3\u679c：<\/p>\n
|