串联机械臂的逆解是一个求解多元非线性方程解的过程。当然,市面上的众多商业臂也都是采用符合pieper准则的构型,可以用解析法或者几何法直接推导出解析式,从而得到封闭解。
而对于不符合peiper准则的臂或者运动学冗余的臂,数值解也是不错的方法。当然,速度肯定是比不上解析法了,而且每次的解只有一个,且每次求解的结果都不一样,当然,好处就是在通用平台上,可以快速搭建机器人系统。
串联机器人运动学方程可写为:
现已知 ,求每个关节的转角 ,由于 带有三角函数,是一个典型的多元非线性问题。可选用Newton Raphson迭代法作为求解方案。
1、Newton Raphson迭代法
把函数 在 点某邻域内进行泰勒展开:
取线性部分(泰勒展开前两项),令其等于0作为 的近似方程:
若 则其解为:
得到迭代关系式:
- 举例
取 :
- 应用在机器人运动学
给定 和初始猜测角度 ( 为空间自由度, 为关节数):
可写成:
,
为一阶偏导,即为雅可比矩阵。
左右同时左乘 :
设 ( 为迭代最小误差,如0.001):
得到:
如果雅可比矩阵不是方阵或机器人位于奇异点,此时用雅可比矩阵的伪逆 .伪逆进行计算.可写成:
实现:
- 求解 处雅克比矩阵:
令 为单位角速度或线速度1时,每个关节对末端的速度旋量
2. 误差项 的求取:
位置误差:
姿态误差:以A为参考,A到B的姿态误差
此时的 需要把旋转矩阵表示成轴角的形式:
最后将 得转换到基于基座标 为参考的姿态误差:
- 代码实现:
int Chain::ikNewton(const Frame &desireFrame, VectorXSDS &outJoints, int maxIter, double eps)
{
if(outJoints.mN != _numOfJoints)
{
return -2;
}
Frame frame;
Twist dtTwist;
MatSDS jac(_numOfJoints,6);
for (int i = 0; i < maxIter; ++i)
{
frame = fk(joints);
dtTwist = Frame::diff(frame, desireFrame);
if(dtTwist.closeToEps(eps))
{
outJoints = joints;
return i;
}
jacobi(jac,joints);
MatSDS dtJoints = jac.pseudoInvert()*dtTwist.toMat();
for (int i = 0; i < joints.mN; ++i)
{
joints[i] = joints[i] + dtJoints[i];
}
}
return -1;
}
- 案例:
- 结果:
对比Msnhnet实现和KDL实现,使用puma560机器人模型,分别测试约20万组点,在0点开始迭代,结果如下。Msnhnet成功率为96.02%, 单次求解速率为114us,KDL成功率为96.11%,单次求解速率为135us。(Win10 I7 10700KF)
- 后续:
后面会采用trac_ik的求解方案进行优化,欢迎继续关注。
- 最后
欢迎关注Msnhnet框架,它是一个深度学习推理框架,也会慢慢变成一个机器人+视觉一体的框架
机器人学建模、规划与控制.Bruno Siciliano
现代机器人学:机构、规划与控制. Kevin M. Lynch.
机器人操作的数学导论.李泽湘
机器人学导论。John J. Craig
评论(0)
您还未登录,请登录后发表或查看评论