Delta机器人逆运动学求解

Delta机器人运动学逆解

前言

由于要用到delta机器人的仿真，但是在网上搜索了很多资料，也搜索了很多论文，找到了很多种不同的求解过程，但是苦于坐标系建立的差别，而且计算方法的差别，得到的运动学逆解很不统一，甚至有很多细节出错的地方，因此为了仿真能够顺利运行，在此处我将记录delta机器人的逆运动学推导过程，即根据给定的(x,y,z),z<0，求解主动臂的角度${\theta }_i$(i = 1,2,3).

逆运动学求解

模型建立

<img src=“/Users/dwl/Desktop/delta/20200628103413343.png”
alt=“20200628103413343” style=“zoom:50%;” />

建立的坐标系如上图所示，其中$C_1{C}_2{C}_3$平面是一个静平台，$A_1{A}_2{A}_3$平面是一个动平台。$C_i{B}_i$是主动臂，其自由度是1，也就是只能绕着$C_i$旋转。而$B_i{A}_i$是从动臂，其自由度是2，其除了能够绕着$B_i$上下转动以外还可以左右摆动，具体想知道实现效果的可以去b站搜索delta机器人，这个问题也是困扰了我很久才知道。而且$C_1{C}_2{C}_3$和$A_1{A}_2{A}_3$永远是平行的，这个根据实验所得，我也不会证明，欢迎各位补充。以下还有一些其他要用到的变量说明，

静平台的半径为R，动平台的半径为r；${\phi }_i$表示$O{C}_i$与坐标系X轴正方向的夹角。该机器人能够主动控制的变量只有${\theta }_i$（${\theta }_i$为0的时候，主动臂与静平台平行，为正时候向下，为负时向上），其末端p点的位置和和从动臂的位姿均是因变量。主动臂的长度为L，从动臂的长度为l。

求解过程

给定末端位姿$x,y,z$和机器人的模型参数，求${\theta }_i,i=1,2,3$.

显然$C_i$的的坐标点容易求得，
$$\begin{array}{c}{C}_i=R\left[\begin{array}{c}\cos {\phi }_i\\ \sin {\phi }_i\\ 0\end{array}\right],{\phi }_i=\frac{2}{3}(i-1)\pi ,i=1,2,3\end{array}$$
由于主动臂$C_i{B}_i$只能绕着$C_i$旋转，且其运动维度只有1，因此$B_i$点的坐标也比较容易确定，
$$\begin{array}{c}{B}_i=R\left[\begin{array}{c}\cos {\phi }_i\\ \sin {\phi }_i\\ 0\end{array}\right]+L\left[\begin{array}{c}\cos {\theta }_i\cos {\phi }_i\\ \cos {\theta }_i\sin {\phi }_i\\ -\sin {\theta }_i\end{array}\right]\end{array}$$
假设 p点在静平台下的坐标为 $(\mathrm{x},\mathrm{y},\mathrm{z})$ 则 $A_i$ 在静平台坐标系下位置矢量为:
$$\begin{array}{c}{A}_i=r\left[\begin{array}{c}\cos {\phi }_i\\ \sin {\phi }_i\\ 0\end{array}\right]+\left[\begin{array}{l}x\\ y\\ z\end{array}\right]\end{array}$$
根据$|B_{i}Ai|=l$，可以使用向量的模长得，

$$\overrightarrow{BA}=\overrightarrow{0A}-\overrightarrow{0B}=r\left[\begin{array}{c}\cos {\phi }_i\\ \sin {\phi }_i\\ 0\end{array}\right]+\left[\begin{array}{l}x\\ y\\ z\end{array}\right]-R\left[\begin{array}{c}\cos {\phi }_i\\ \sin {\phi }_i\\ 0\end{array}\right]-L\left[\begin{array}{c}\cos {\theta }_i\cos {\phi }_i\\ \cos {\theta }_i\sin {\phi }_i\\ -\sin {\theta }_i\end{array}\right]$$

$$\begin{array}{rl}|\overrightarrow{BA}{|}^2=l^2=& (r\cos {\phi }_i+x-R\cos {\phi }_i-L\cos {\theta }_i\cos {\phi }_i{)}^2\\ & +(r\sin {\phi }_i+y-R\sin {\phi }_i-L\cos {\theta }_i\sin {\phi }_i{)}^2\\ & +(z+\sin {\theta }_i{)}^2\end{array}$$

因此可以化简为，
$$\begin{array}{c}{\left[\left(R-r+L\cos {\theta }_i\right)\cos {\phi }_i-x\right]}^2+{\left[\left(R-r+L\cos {\theta }_i\right)\sin {\phi }_i-y\right]}^2+{\left(L\sin {\theta }_i+z\right)}^2=l^2\end{array}$$

此时$R,r,l,L,{\phi }_i,x,y,z$均为已知量，只有${\theta }_i$是待求变量。

接下来开始这个一大串式子的求解，为了求解过程更加直观，我先求前面两个平方的和，
$$\begin{array}{c}{\left[\left(R-r+L\cos {\theta }_i\right)\cos {\phi }_i-x\right]}^2+{\left[\left(R-r+L\cos {\theta }_i\right)\sin {\phi }_i-y\right]}^2=\\ （为了简化运算我将两条式子展开对齐）\\ ((R-r+L\cos {\theta }_i{)}^2{\cos }^2{\phi }_i-2x(R-r+L\cos {\theta }_i)\cos {\phi }_i+x^2+\\ ((R-r+L\cos {\theta }_i{)}^2{\sin }^2{\phi }_i-2y(R-r+L\cos {\theta }_i)\sin {\phi }_i+y^2\end{array}$$
显然根据$co{s}^2{\phi }_i+si{n}^2{\phi }_i=1$，并且把待求变量${\theta }_i$移出到括号外有，
$$\begin{array}{rl}前面两项平方& =(R-r+L\cos {\theta }_i{)}^2-2(x\cos {\phi }_i+y\sin {\phi }_i)(R-r)\\ & -2L(x\cos {\phi }_i+y\sin {\phi }_i)\cos {\theta }_i+x^2+y^2\\ & =(R-r{)}^2+2L(R-r)\cos {\theta }_i+L^2{\cos }^2{\theta }_i-2(x\cos {\phi }_i+y\sin {\phi }_i)(R-r)\\ & -2L(x\cos {\phi }_i+y\sin {\phi }_i)\cos {\theta }_i+x^2+y^2\end{array}$$
再加上第三个平方项，
$$\begin{gathered} l^2=(R-r{)}^2+2L(R-r)\cos {\theta }_i+L^2{\cos }^2{\theta }_i-2(x\cos {\phi }_i+y\sin {\phi }_i)(R-r) \\ -2L(x\cos {\phi }_i+y\sin {\phi }_i)\cos {\theta }_i+x^2+y^2+z^2+L^2{\sin }^2{\theta }_i+2Lz\sin {\theta }_i \end{gathered}$$
注意到$L^2{\sin }^2{\theta }_i+L^2{\cos }^2{\theta }_i=L^2$，合并得，
$$\begin{gathered} l^2=(R-r{)}^2+L^2+x^2+y^2+z^2-2(x\cos {\phi }_i+y\sin {\phi }_i)(R-r) \\ +2L(R-r-x\cos {\phi }_i-y\sin {\phi }_i)\cos {\theta }_i+2Lz\sin {\theta }_i \end{gathered}$$
由于$R,r,l,L,{\phi }_i,x,y,z$均为已知量，故使得
$$\begin{array}{rl}& A=2L(R-r-x\cos {\phi }_i-y\sin {\phi }_i)\\ & B=2Lz\\ & C=(R-r{)}^2+L^2+x^2+y^2+z^2-2(x\cos {\phi }_i+y\sin {\phi }_i)(R-r)-l^2\end{array}$$
那么原式就可以化成，
$$A\cos {\theta }_i+B\sin {\theta }_i+C=0$$
其中A,B,C均是常数。再带入万能公式，
$$\begin{array}{c}\sin {\theta }_i=\frac{2t}{1+t^2},\cos {\theta }_i=\frac{1-t^2}{1+t^2},其中t=\tan \frac{{\theta }_i}{2}\end{array}$$
得到，

$$(C-A)t^2+2Bt+(A+C)=0$$
解得方程为，
$$t=\frac{-2B-\sqrt{4B^2-4(C^2-A^2)}}{2(C-A)}=\frac{-B-\sqrt{A^2+B^2-C^2}}{C-A}$$
其中，
$$\{\begin{array}{ll}& A=2L(R-r-x\cos {\phi }_i-y\sin {\phi }_i)\\ & B=2Lz\\ & C=(R-r{)}^2+L^2+x^2+y^2+z^2-2(x\cos {\phi }_i+y\sin {\phi }_i)(R-r)-l^2\end{array}$$

具体的cpp实现代码如下，

double R = 0.065;
double r = 0.02;
double L = 0.105;
double l = 0.130;
bool DeltaInversekinematic(double x, double y, double z, double& theta1, double& theta2, double& theta3) {
        double phi_list[3] = {0,2.0/3.0*PI,4.0/3.0*PI};
        double theta_list[3];
        for (int i=0;i<3;i++){
            double phi = phi_list[i];
            double a = 2*L*(R-r-x*cos(phi)-y*sin(phi));
            double b = 2*L*z;
            double c = x*x+y*y+z*z+L*L+(R-r)*(R-r)-l*l-2*(R-r)*(x*cos(phi)+y*sin(phi));
            theta_list[i] = (-b-sqrt(a*a+b*b-c*c))/(c-a);
        }
        theta1 = theta_list[0];
        theta2 = theta_list[1];
        theta3 = theta_list[2];
        if (std::isnan(theta1)||std::isnan(theta2)||std::isnan(theta3)){
            std::cout<<"No feasible solution!"<<std::endl;
        return false;
    }
    std::cout<<"The calculated angle is"<<theta1<<","<<theta2<<","<<theta3<<"."<<std::endl;
    return true;
} 

参考文章：
https://blog.csdn.net/qq413886183/article/details/106993725
https://www.doc88.com/p-20559413883394.html