MIT Cheetah WBC中不同优先级任务的执行零空间方法

最新推荐文章于 2024-07-01 22:56:08 发布

Na-Cl

最新推荐文章于 2024-07-01 22:56:08 发布

阅读量1.1k

点赞数 3

分类专栏：四足机器人文章标签：算法运动学

本文链接：https://blog.csdn.net/qq_29735067/article/details/125865603

版权

四足机器人专栏收录该内容

7 篇文章 26 订阅

订阅专栏

本文主要参考：请结合参考文章进行阅读(43条消息) 四足机器人中不同优先级任务的执行——Null-Space Projection方法_robotics_notes的博客-CSDN博客

本文仅个人理解，如有错漏，请在评论指出，感谢！

下文中等式右侧的 $\dot{q_{i}}\rightarrow i=1\sim k$ 应指代为方程组特解

MIT的任务设置：接触腿控制为第一优先级(第一任务)机身姿态机身位置腿部位置依次排列

对其中三个任务的推导进行补充

此式 $\dot{z_{2}}$ 为特解，遂在这里补充为通解

$\dot{z_{2}}=\left [ J_{2} (I-J^{+}_{1}J_{1})\right ]^{+}(\dot{x_{2}}-J_{2} J^{+}_{1}\dot{x_{1}})+N_{21}\dot{z_{2}} {\color{Red} \rightharpoonup (5)}$

$N_{21}=Nullproject(J_{2} (I-J^{+}_{1}J_{1}))=Nullproject(J_{21})$

将(5)式替换(4)式回代到(2)

$\\\dot{z_{2}}=\left [ J_{2} (I-J^{+}_{1}J_{1})\right ]^{+}(\dot{x_{2}}-J_{2} J^{+}_{1}\dot{x_{1}})+N_{21}\dot{z_{3}} \\ \dot{q_{2}}=J^{+}_{1}\dot{x_{1}}+(I-J^{+}_{1}J_{1})\left [ J_{2} (I-J^{+}_{1}J_{1})\right ]^{+}(\dot{x_{2}}-J_{2} J^{+}_{1}\dot{x_{1}})+(I-J^{+}_{1}J_{1})(N_{21}\dot{z_{3}}) \\\dot{q_{2}}= J^{+}_{1}\dot{x_{1}}+N_{1}\left [ J_{2} N_{1}\right ]^{+}(\dot{x_{2}}-J_{2} \dot{q_{1}})+N_{1}(N_{21}\dot{z_{3}}) \\\dot{q_{2}}= J^{+}_{1}\dot{x_{1}}+\left [ J_{2} N_{1}\right ]^{+}(\dot{x_{2}}-J_{2} \dot{q_{1}})+ N_{1}(N_{21}\dot{z_{3}}){\color{Red} \rightarrow (6)}$

此时要满足 $J_{3}\dot{q}=\dot{x_{3}}$ 参考计算 $\dot{q_{2}}$ 的方法将 $\dot{q_{2}}$ 带入

$\\J_{3}\dot{q_{2}}= J_{3}J^{+}_{1}\dot{x_{1}}+J_{3}\left [ J_{2} N_{1}\right ]^{+}(\dot{x_{2}}-J_{2} \dot{q_{1}})+J_{3}N_{1}(N_{21}\dot{z_{3}})=\dot{x_{3}} \\\dot{z_{3}}=\left [ J_{3}N_{1}N_{21} \right ]^{+}[\dot{x_{3}}-[J_{3}J^{+}_{1}\dot{x_{1}}+J_{3}\left [ J_{2} N_{1}\right ]^{+}(\dot{x_{2}}-J_{2} \dot{q_{1}})]]+N_{32}\dot{z_{4}} \\\dot{z_{3}}=\left [ J_{3}N_{1}N_{21} \right ]^{+}[\dot{x_{3}}-J_{3}\dot{q_{2}}]+N_{32}\dot{z_{4}} {\color{Red} \rightarrow (7)} \\N_{32}=Nullproject(J_{3}N_{1}N_{21})=Nullproject(J_{32})$

将上式(7)带入式（6）

$\\\dot{q_{3}}= J^{+}_{1}\dot{x_{1}}+N_{1}\left [ J_{2} N_{1}\right ]^{+}(\dot{x_{2}}-J_{2} \dot{q_{1}})+N_{1}N_{21}\left [ J_{3}N_{1}N_{21} \right ]^{+}[\dot{x_{3}}-J_{3}\dot{q_{2}}]+N_{1}N_{21}N_{32}\dot{z_{4}} ) \\\dot{q_{3}}=\dot{q_{2}} +N_{1}N_{21}\left [ J_{3}N_{1}N_{21} \right ]^{+}[\dot{x_{3}}-J_{3}\dot{q_{2}}]+N_{1}N_{21}N_{32}\dot{z_{4}} \\\dot{q_{3}}=\dot{q_{2}} +\left [ J_{3}N_{1}N_{21} \right ]^{+}[\dot{x_{3}}-J_{3}\dot{q_{2}}]+N_{1}N_{21}N_{32}\dot{z_{4}} {\color{Red} \rightarrow (8)}$

同理:要满足 $J_{4}\dot{q}=\dot{x_{4}}$ 将式(8)带入可得式子 $\dot{z_{4}}$

$\\\dot{z_{4}}=\left [ J_{4}N_{1}N_{21}N_{32} \right ]^{+}[\dot{x_{4}}-J_{4}\dot{q_{3}}]+N_{43}\dot{z_{5}} {\color{Red} \rightarrow (9)} \\N_{43}=Nullproject(J_{4}N_{1}N_{21}N_{32})=Nullproject(J_{43})$

将上式(9)带入式（8）

$\\\dot{q_{4}}=\dot{q_{2}} +\left [ J_{3}N_{1}N_{21}N_{32} \right ]^{+}[\dot{x_{4}}-J_{4}\dot{q_{3}}]+N_{1}N_{21}N_{32}N_{43}\dot{z_{4}}$

综上：

$\\\dot{q_{1}}=J^{+}_{1}\dot{x_{1}} \\\dot{q_{2}}= \dot{q_{1}}+\left [ J_{2} N_{1}\right ]^{+}(\dot{x_{2}}-J_{2} \dot{q_{1}}) \\\dot{q_{3}}=\dot{q_{2}} +\left [ J_{3}N_{1}N_{21} \right ]^{+}[\dot{x_{3}}-J_{3}\dot{q_{2}}] \\\dot{q_{4}}=\dot{q_{2}} +\left [ J_{3}N_{1}N_{21}N_{32} \right ]^{+}[\dot{x_{4}}-J_{4}\dot{q_{3}}]$