复现KDL:使用Msnhnet实现串联机器人运动数值逆解(一）

串联机械臂的逆解是一个求解多元非线性方程解的过程。当然，市面上的众多商业臂也都是采用符合pieper准则的构型，可以用解析法或者几何法直接推导出解析式，从而得到封闭解。

而对于不符合peiper准则的臂或者运动学冗余的臂，数值解也是不错的方法。当然，速度肯定是比不上解析法了，而且每次的解只有一个，且每次求解的结果都不一样，当然，好处就是在通用平台上，可以快速搭建机器人系统。

串联机器人运动学方程可写为：
p=f(Θ),Θ=[θ1,θ2...θn]Tp=f(\Theta), \Theta=[\theta_1,\theta_2...\theta_n]^Tp=f(Θ),Θ=[θ1,θ2...θn]T
现已知 ppp ,求每个关节的转角Θ\ThetaΘ ,由于f(Θ)f(\Theta)f(Θ)带有三角函数，是一个典型的多元非线性问题。可选用Newton Raphson迭代法作为求解方案。

1、Newton Raphson迭代法
把函数 f(x)f(x)f(x) 在 x0x_0x0 点某邻域内进行泰勒展开：
f(x)=f(x0)+f’(x0)(x−x0)1!+⋯+f(n)(x0)(x−x0)nn!+Rn(x)f(x)=f(x_0)+\frac{f’(x_0)(x−x_0)}{1!}+⋯+\frac{f^{(n)}(x_0)(x−x_0)^n}{n!}+R_n(x) f(x)=f(x0)+1!f’(x0)(x−x0)+⋯+n!f(n)(x0)(x−x0)n+Rn(x)
取线性部分(泰勒展开前两项),令其等于0作为f(x)=0f(x)=0f(x)=0 的近似方程:
f(x0)+f’(x0)(x−x0)=0f(x_0)+f’(x_0)(x−x_0)=0f(x0)+f’(x0)(x−x0)=0
若f’(x0)≠0f’(x_0)≠0f’(x0)=0, 则其解为:
x1=x0−f(x0)f’(x0)x_1=x_0−\frac{f(x_0)}{f’(x_0)}x1=x0−f’(x0)f(x0)
得到迭代关系式:
xn+1=xn−f(xn)f’(xn)x_{n+1}=x_n−\frac{f(x_n)}{f’(x_n)}xn+1=xn−f’(xn)f(xn)
- 举例
f(x)=x3−6x,取x0=1f(x)=x^3−6x, 取 x_0=1f(x)=x3−6x,取x0=1

x1=x0−x03−6x03x02−6=−23,f(x1)=10027x_1=x_0−x_0^3−\frac{6x_0}{3x_0^2}−6=−\frac{2}{3}, f(x_1)=\frac{100}{27}x1=x0−x03−3x026x0−6=−32,f(x1)=27100

x2=x1−x13−6x13x12−6=863,f(x2)=0.759857x_2=x_1−x_1^3−\frac{6x_1}{3x_1^2}−6=\frac{8}{63}, f(x_2)=0.759857x2=x1−x13−3x126x1−6=638,f(x2)=0.759857

x3=x2−x23−6x23x22−6=−0.000688086,f(x3)=0.00412852x_3=x_2−x_2^3−\frac{6x_2}{3x_2^2}−6=−0.000688086,f(x_3)=0.00412852x3=x2−x23−3x226x2−6=−0.000688086,f(x3)=0.00412852

x4=x3−x33−6x33x32−6=1.08594×10−10,f(x4)=−6.51564×10−10x_4=x_3−x_3^3−\frac{6x_3}{3x_3^2}−6=1.08594×10^{−10},f(x_4)=−6.51564×10^{−10}x4=x3−x33−3x326x3−6=1.08594×10−10,f(x4)=−6.51564×10−10

- 应用在机器人运动学
给定 p∈Rmp∈ℝ^mp∈Rm 和初始猜测角度Θ0∈Rn\Theta^0∈ℝ^nΘ0∈Rn( mmm 为空间自由度,nnn 为关节数):
p=f(Θ)=f(Θi)+∂f∂Θ∣Θi(Θ−Θi)+Rn(Θ)p=f(\Theta)=f(\Theta^i)+\frac{\partial{f}}{\partial{\Theta}}|_{\Theta^i}(\Theta−\Theta^i)+R_n(\Theta)p=f(Θ)=f(Θi)+∂Θ∂f∣Θi(Θ−Θi)+Rn(Θ)
可写成：
p−f(Θi)=J(Θi)ΔΘ,J(Θi)=[J1,J2...,Jn]p−f(\Theta^i)=J(\Theta^i)\Delta\Theta , J(\Theta^i)=[J_1,J_2...,J_n] p−f(Θi)=J(Θi)ΔΘ,J(Θi)=[J1,J2...,Jn]
J(Θi)J(\Theta^i)J(Θi)为一阶偏导，即为雅可比矩阵。
左右同时左乘 J−1(Θi)J^{−1}(\Theta^i)J−1(Θi) :
J−1(Θi)(p−f(Θi))=ΔΘJ^{−1}(\Theta^i)(p−f(\Theta^i))=\Delta\ThetaJ−1(Θi)(p−f(Θi))=ΔΘ
设 ‖e‖<ε‖e‖<ε‖e‖<ε ( εεε 为迭代最小误差,如0.001):
e=(p−f(Θi))e=(p−f(\Theta^i))e=(p−f(Θi))

ΔΘ=Θi+1−Θi\Delta\Theta=\Theta^{i+1}-\Theta^{i}ΔΘ=Θi+1−Θi
得到：
Θi+1=Θi+J−1(Θi)e\color{red}{\Theta^{i+1}=\Theta^i+J^{-1}(\Theta^i)e }Θi+1=Θi+J−1(Θi)e
如果雅可比矩阵不是方阵或机器人位于奇异点，此时用雅可比矩阵的伪逆 J†(Θi)J^†(\Theta^i)J†(Θi) .伪逆进行计算.可写成：
Θi+1=Θi+J†(Θi)e\color{red}{\Theta^{i+1}=\Theta^i+J^†(\Theta^i)e }Θi+1=Θi+J†(Θi)e
- 实现：

求解 Θi\Theta^{i}Θi 处雅克比矩阵：
令 Θi=[θ1i,θ2i...θni]\Theta^{i}=[\theta_1^i,\theta_2^i...\theta_n^i]Θi=[θ1i,θ2i...θni]为单位角速度或线速度1时，每个关节对末端的速度旋量
Ji=Twisti=[vxi,vyi,vzi,rxi,ryi,rzi]TJ(Θi)=[J1,J2...,Jn]J_i=Twist_i=[v_{x_i},v_{y_i},v_{z_i},r_{x_i},r_{y_i},r_{z_i}]^T \\J(\Theta^i)=[J_1,J_2...,J_n]Ji=Twisti=[vxi,vyi,vzi,rxi,ryi,rzi]TJ(Θi)=[J1,J2...,Jn]
误差项 eee 的求取：

e=[epeω]=[ΔxΔyΔzΔrxΔryΔrz]e=[\begin{matrix}e_p&e_\omega\end{matrix}]=[\begin{matrix}\Delta x&\Delta y&\Delta z&\Delta rx&\Delta ry&\Delta rz\end{matrix}]e=[epeω]=[ΔxΔyΔzΔrxΔryΔrz]
位置误差: ep=PB−PA=[xb−xayb−yazb−za]e_p=P_B−P_A=[\begin{matrix}x_b−x_a&y_b−y_a&z_b−z_a\end{matrix}]ep=PB−PA=[xb−xayb−yazb−za]
姿态误差：以AAA为参考，AAA到BBB的姿态误差
erotA2B=Rbase2A−1∗Rbase2Be_{rot_{A2B}}=R_{base2A}^{−1}∗R_{base2B}erotA2B=Rbase2A−1∗Rbase2B
此时的 erotA2Be_{rot_{A2B}}erotA2B 需要把旋转矩阵表示成轴角的形式：
erotA2B⇒eωA2Be_{rot_{A2B}} \Rightarrow e_{\omega_{A2B}}erotA2B⇒eωA2B
最后将eωA2Be_{ω_{A2B}}eωA2B得转换到基于基座标 basebasebase 为参考的姿态误差:
eω=Rbase2A∗eωA2Be_ω=R_{base2A}∗e_{ω_{A2B}}eω=Rbase2A∗eωA2B

- 代码实现：源码

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框架，它是一个深度学习推理框架，也会慢慢变成一个机器人+视觉一体的框架
Msnhnet
知乎介绍

机器人学建模、规划与控制.Bruno Siciliano
现代机器人学：机构、规划与控制. Kevin M. Lynch.
机器人操作的数学导论.李泽湘
机器人学导论。John J. Craig

复现KDL:使用Msnhnet实现串联机器人运动数值逆解(一）相关推荐

文献阅读1：基于旋量理论的串联机器人运动特性研究现状
从机构学与机器人学的发展历史上来看,机构学与机器人学的发展与数学工具是息息相关的.与机构学与机器人学联系紧密的数学工具有很多:如传统意义上的线性代数与矩阵理论.用于拓扑结构分析及综合的图论(grap ...
仿斯坦福四足机器人的运动学逆解
1 引言仿斯坦福四足机器人的软件流程如下图所示.其中运动学逆解直接输出给舵机,控制机器人的运动,因此运动学逆解很重要. 2 基本概念 2.1机械结构模型对于8自由度机器人,其机械结构模型如 ...
(6.1)MATLAB机器人正、逆解中姿态求解的欧拉角的说明
1.欧拉角的类型: 例程中MATLAB机器人工具箱中采用的是世界坐标系的欧拉角(先旋转的在右边).如下: 利用机器人工具箱的.teach()函数进行可是化时的欧拉角如下: 2.正解算法中的欧拉角: 在 ...
工程文件参考——STM32F4+幻尔LSC舵机控制板四足机器人足端逆解控制器
文章目录代码实现 Quadruped_huaner.h Quadruped_huaner.c # 前言本文主要提供使用F4系列芯片针对幻尔LSC舵机控制板的四足机器人足端坐标逆解库,供各位参考请 ...
Puma560机器人运动学正逆解
puma560机器人D-H参数 puma560采用的是改进D-H参数,其DH参数表如下: i αi ai di θi 1 0 0 0 t1 2 -90 0 0 t2 3 0 r2 d3 t3 4 -9 ...
如何拖动末端使机器人运动仿真-使用Peter机器人工具箱
1.前记这里这是一个简单的分享,一种在机器人模型末端添加一个marker并进行拖动的仿真.类似于机器人系统工具箱中的interactiveRigidBodyTree函数功能,不过简单了功能许多.这里 ...
article-六轴码垛机器人admas正逆运动学仿真
基座自由度.大臂摆动自由度.小臂摆动自由度.腕部Y轴摆动自由度.腕部Z轴摆动自由度及其腕部末端X轴旋转自由度其导入过程为: 机器人三维模型总体有6个部分.打开机器人的SolidWork三维模型,依次 ...
机器人运动学逆解中最常用的三角方程(附代码)
文章目录一.推导步骤 1.方法1 2.方法2 二.实例(以SCARA机器人逆解为例) 在推导机器人运动学逆解的解析解时,经常会遇到以下三角方程: k1sin(θ)+k2cos(θ)=k3(1)k ...
MATLAB机器人正逆解
MATLAB机器人求正逆解手把手教你MATLAB Robotics Toolbox工具箱③ Matlab RoboticToolBox(一)Link参数.三自由度/四自由度逆运动学 https:// ...
工业机器人（11）-串联机器人、并联机器人和混联机器人
机器人从机构学的角度可以分为串联机器人和并联机器人两大类.串联机器人以开环机构为机器人机构原型:并联机器人为有一个或几个闭环组成的关节点坐标相互关联的机器人. 串联机器人串联机器人是一种开式运动链机 ...

复现KDL:使用Msnhnet实现串联机器人运动数值逆解(一）

复现KDL:使用Msnhnet实现串联机器人运动数值逆解(一）相关推荐

最新文章

热门文章