3
时间约束和kinematics约束及雅格比矩阵的推导
source link: https://charon-cheung.github.io/2023/03/04/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/TEB%E7%AE%97%E6%B3%95/%E6%97%B6%E9%97%B4%E7%BA%A6%E6%9D%9F%E5%92%8Ckinematics%E7%BA%A6%E6%9D%9F%E5%8F%8A%E9%9B%85%E6%A0%BC%E6%AF%94%E7%9F%A9%E9%98%B5%E7%9A%84%E6%8E%A8%E5%AF%BC/#%E6%97%B6%E9%97%B4%E7%BA%A6%E6%9D%9F
Go to the source link to view the article. You can view the picture content, updated content and better typesetting reading experience. If the link is broken, please click the button below to view the snapshot at that time.
时间约束和kinematics约束及雅格比矩阵的推导
EdgeTimeOptimal类太简单了，误差函数 _error[0] = timediff->dt();，优化 ΔTiΔTi，那么对其求偏导，显然只有一个矩阵，一个元素 1 ： _jacobianOplusXi( 0 , 0 ) = 1;
kinematics 约束
两个误差方程，一个是 holonomic约束，一个是 positive-drive-direction约束 ：
|(cosθ1+cosθ2)(y2−y1)−(sinθ1+sinθ2)(x2−x1)||(cosθ1+cosθ2)(y2−y1)−(sinθ1+sinθ2)(x2−x1)|
−(x2−x1)cosθ1−(y2−y1)sinθ1−(x2−x1)cosθ1−(y2−y1)sinθ1
两个configure，所以两个雅格比，维度明显是 2x3，两个误差方程分别对(x, y, angle)求偏导，源码里的求导很简单，还不如速度约束的求导复杂，就不写过程了。
值得注意的是绝对值的求导结果会用sign函数表示。
Recommend
About Joyk
Aggregate valuable and interesting links.
Joyk means Joy of geeK