Delta机器人运动学逆解
前言
由于要用到delta机器人的仿真,但是在网上搜索了很多资料,也搜索了很多论文,找到了很多种不同的求解过程,但是苦于坐标系建立的差别,而且计算方法的差别,得到的运动学逆解很不统一,甚至有很多细节出错的地方,因此为了仿真能够顺利运行,在此处我将记录delta机器人的逆运动学推导过程,即根据给定的(x,y,z),z<0,求解主动臂的角度 θ i \theta_i θi(i = 1,2,3).
逆运动学求解
模型建立
<img src=“/Users/dwl/Desktop/delta/20200628103413343.png”
alt=“20200628103413343” style=“zoom:50%;” />
建立的坐标系如上图所示,其中 C 1 C 2 C 3 C_1C_2C_3 C1C2C3平面是一个静平台, A 1 A 2 A 3 A_1A_2A_3 A1A2A3平面是一个动平台。 C i B i C_iB_i CiBi是主动臂,其自由度是1,也就是只能绕着 C i C_i Ci旋转。而 B i A i B_iA_i BiAi是从动臂,其自由度是2,其除了能够绕着 B i B_i Bi上下转动以外还可以左右摆动,具体想知道实现效果的可以去b站搜索delta机器人,这个问题也是困扰了我很久才知道。而且 C 1 C 2 C 3 C_1C_2C_3 C1C2C3和 A 1 A 2 A 3 A_1A_2A_3 A1A2A3永远是平行的,这个根据实验所得,我也不会证明,欢迎各位补充。以下还有一些其他要用到的变量说明,
静平台的半径为R,动平台的半径为r; φ i \varphi_i φi表示 O C i OC_i OCi与坐标系X轴正方向的夹角。该机器人能够主动控制的变量只有 θ i \theta_i θi( θ i \theta_i θi为0的时候,主动臂与静平台平行,为正时候向下,为负时向上),其末端p点的位置和和从动臂的位姿均是因变量。主动臂的长度为L,从动臂的长度为l。
求解过程
给定末端位姿 x , y , z x,y,z x,y,z和机器人的模型参数,求 θ i , i = 1 , 2 , 3 \theta_i,i=1,2,3 θi,i=1,2,3.
显然
C
i
C_i
Ci的的坐标点容易求得,
C
i
=
R
[
cos
φ
i
sin
φ
i
0
]
,
φ
i
=
2
3
(
i
−
1
)
π
,
i
=
1
,
2
,
3
\begin{equation*} C_i=R\left[\begin{array}{c} \cos \varphi_i \\ \sin \varphi_i \\ 0 \end{array}\right], \varphi_i=\frac{2}{3}(i-1) \pi, i=1,2,3 \end{equation*}
Ci=R
cosφisinφi0
,φi=32(i−1)π,i=1,2,3
由于主动臂
C
i
B
i
C_iB_i
CiBi只能绕着
C
i
C_i
Ci旋转,且其运动维度只有1,因此
B
i
B_i
Bi点的坐标也比较容易确定,
B
i
=
R
[
cos
φ
i
sin
φ
i
0
]
+
L
[
cos
θ
i
cos
φ
i
cos
θ
i
sin
φ
i
−
sin
θ
i
]
\begin{equation*} B_i=R\left[\begin{array}{c} \cos \varphi_i \\ \sin \varphi_i \\ 0 \end{array}\right]+L\left[\begin{array}{c} \cos \theta_i \cos \varphi_i \\ \cos \theta_i \sin \varphi_i \\ -\sin \theta_i \end{array}\right] \end{equation*}
Bi=R
cosφisinφi0
+L
cosθicosφicosθisinφi−sinθi
假设 p点在静平台下的坐标为
(
x
,
y
,
z
)
(\mathrm{x}, \mathrm{y}, \mathrm{z})
(x,y,z) 则
A
i
A_i
Ai 在静平台坐标系下位置矢量为:
A
i
=
r
[
cos
φ
i
sin
φ
i
0
]
+
[
x
y
z
]
\begin{equation*} A_i=r\left[\begin{array}{c} \cos \varphi_i \\ \sin \varphi_i \\ 0 \end{array}\right]+\left[\begin{array}{l} x \\ y \\ z \end{array}\right] \end{equation*}
Ai=r
cosφisinφi0
+
xyz
根据
∣
B
i
A
i
∣
=
l
|B_iAi|=l
∣BiAi∣=l,可以使用向量的模长得,
B A ⃗ = 0 A ⃗ − 0 B ⃗ = r [ cos φ i sin φ i 0 ] + [ x y z ] − R [ cos φ i sin φ i 0 ] − L [ cos θ i cos φ i cos θ i sin φ i − sin θ i ] \vec{BA} = \vec{0A} - \vec{0B}=r\left[\begin{array}{c} \cos \varphi_i \\ \sin \varphi_i \\ 0 \end{array}\right]+\left[\begin{array}{l} x \\ y \\ z \end{array}\right] -R\left[\begin{array}{c} \cos \varphi_i \\ \sin \varphi_i \\ 0 \end{array}\right]-L\left[\begin{array}{c} \cos \theta_i \cos \varphi_i \\ \cos \theta_i \sin \varphi_i \\ -\sin \theta_i \end{array}\right] BA=0A−0B=r cosφisinφi0 + xyz −R cosφisinφi0 −L cosθicosφicosθisinφi−sinθi
∣ B A ⃗ ∣ 2 = l 2 = ( r cos φ i + x − R cos φ i − L cos θ i cos φ i ) 2 + ( r sin φ i + y − R sin φ i − L cos θ i sin φ i ) 2 + ( z + sin θ i ) 2 \begin{align*} |\vec{BA}|^2 = l^2= &(r\cos\varphi_i+x-R\cos\varphi_i-L\cos \theta_i \cos \varphi_i)^2\\ &+ (r\sin\varphi_i+y-R\sin\varphi_i-L\cos \theta_i \sin \varphi_i)^2\\ &+(z+\sin \theta_i)^2 \end{align*} ∣BA∣2=l2=(rcosφi+x−Rcosφi−Lcosθicosφi)2+(rsinφi+y−Rsinφi−Lcosθisinφi)2+(z+sinθi)2
因此可以化简为,
[
(
R
−
r
+
L
cos
θ
i
)
cos
φ
i
−
x
]
2
+
[
(
R
−
r
+
L
cos
θ
i
)
sin
φ
i
−
y
]
2
+
(
L
sin
θ
i
+
z
)
2
=
l
2
\begin{equation*} {\left[\left(R-r+L \cos \theta_i\right) \cos \varphi_i-x\right]^2+} {\left[\left(R-r+L \cos \theta_i\right) \sin \varphi_i-y\right]^2+} \left(L \sin \theta_i+z\right)^2 = l^2 \end{equation*}
[(R−r+Lcosθi)cosφi−x]2+[(R−r+Lcosθi)sinφi−y]2+(Lsinθi+z)2=l2
此时 R , r , l , L , φ i , x , y , z R,r,l,L,\varphi_i,x,y,z R,r,l,L,φi,x,y,z均为已知量,只有 θ i \theta_i θi是待求变量。
接下来开始这个一大串式子的求解,为了求解过程更加直观,我先求前面两个平方的和,
[
(
R
−
r
+
L
cos
θ
i
)
cos
φ
i
−
x
]
2
+
[
(
R
−
r
+
L
cos
θ
i
)
sin
φ
i
−
y
]
2
=
(为了简化运算我将两条式子展开对齐)
(
(
R
−
r
+
L
cos
θ
i
)
2
cos
2
φ
i
−
2
x
(
R
−
r
+
L
cos
θ
i
)
cos
φ
i
+
x
2
+
(
(
R
−
r
+
L
cos
θ
i
)
2
sin
2
φ
i
−
2
y
(
R
−
r
+
L
cos
θ
i
)
sin
φ
i
+
y
2
\begin{gather} \left[\left(R-r+L \cos \theta_i\right) \cos \varphi_i-x\right]^2+ \left[\left(R-r+L \cos \theta_i\right) \sin \varphi_i-y\right]^2 = \\ (为了简化运算我将两条式子展开对齐) \\ ((R-r+L\cos \theta_i)^2\cos^2\varphi_i - 2x(R-r+L\cos\theta_i)\cos\varphi_i+x^2+\\ ((R-r+L\cos \theta_i)^2\sin^2\varphi_i - 2y(R-r+L\cos\theta_i)\sin\varphi_i+y^2 \end{gather}
[(R−r+Lcosθi)cosφi−x]2+[(R−r+Lcosθi)sinφi−y]2=(为了简化运算我将两条式子展开对齐)((R−r+Lcosθi)2cos2φi−2x(R−r+Lcosθi)cosφi+x2+((R−r+Lcosθi)2sin2φi−2y(R−r+Lcosθi)sinφi+y2
显然根据
c
o
s
2
φ
i
+
s
i
n
2
φ
i
=
1
cos^2\varphi_i+sin^2\varphi_i = 1
cos2φi+sin2φi=1,并且把待求变量
θ
i
\theta_i
θi移出到括号外有,
前面两项平方
=
(
R
−
r
+
L
cos
θ
i
)
2
−
2
(
x
cos
φ
i
+
y
sin
φ
i
)
(
R
−
r
)
−
2
L
(
x
cos
φ
i
+
y
sin
φ
i
)
cos
θ
i
+
x
2
+
y
2
=
(
R
−
r
)
2
+
2
L
(
R
−
r
)
cos
θ
i
+
L
2
cos
2
θ
i
−
2
(
x
cos
φ
i
+
y
sin
φ
i
)
(
R
−
r
)
−
2
L
(
x
cos
φ
i
+
y
sin
φ
i
)
cos
θ
i
+
x
2
+
y
2
\begin{align*} 前面两项平方 &= (R-r+L\cos \theta_i)^2-2(x\cos\varphi_i+y\sin\varphi_i)(R-r)\\ &\space\space\space\space-2L(x\cos\varphi_i+y\sin\varphi_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\varphi_i+y\sin\varphi_i)(R-r)\\ &\space\space\space\space-2L(x\cos\varphi_i+y\sin\varphi_i)\cos\theta_i+x^2+y^2 \end{align*}
前面两项平方=(R−r+Lcosθi)2−2(xcosφi+ysinφi)(R−r) −2L(xcosφi+ysinφi)cosθi+x2+y2=(R−r)2+2L(R−r)cosθi+L2cos2θi−2(xcosφi+ysinφi)(R−r) −2L(xcosφi+ysinφi)cosθi+x2+y2
再加上第三个平方项,
l
2
=
(
R
−
r
)
2
+
2
L
(
R
−
r
)
cos
θ
i
+
L
2
cos
2
θ
i
−
2
(
x
cos
φ
i
+
y
sin
φ
i
)
(
R
−
r
)
−
2
L
(
x
cos
φ
i
+
y
sin
φ
i
)
cos
θ
i
+
x
2
+
y
2
+
z
2
+
L
2
sin
2
θ
i
+
2
L
z
sin
θ
i
l^2=(R-r)^2+2L(R-r)\cos \theta_i+L^2\cos^2\theta_i-2(x\cos\varphi_i+y\sin\varphi_i)(R-r) \\-2L(x\cos\varphi_i+y\sin\varphi_i)\cos\theta_i +x^2+y^2+z^2 +L^2\sin^2\theta_i+2Lz\sin\theta_i
l2=(R−r)2+2L(R−r)cosθi+L2cos2θi−2(xcosφi+ysinφi)(R−r)−2L(xcosφi+ysinφi)cosθi+x2+y2+z2+L2sin2θi+2Lzsinθi
注意到
L
2
sin
2
θ
i
+
L
2
cos
2
θ
i
=
L
2
L^2\sin^2\theta_i+L^2\cos^2\theta_i=L^2
L2sin2θi+L2cos2θi=L2,合并得,
l
2
=
(
R
−
r
)
2
+
L
2
+
x
2
+
y
2
+
z
2
−
2
(
x
cos
φ
i
+
y
sin
φ
i
)
(
R
−
r
)
+
2
L
(
R
−
r
−
x
cos
φ
i
−
y
sin
φ
i
)
cos
θ
i
+
2
L
z
sin
θ
i
l^2=(R-r)^2+L^2+x^2+y^2+z^2-2(x\cos\varphi_i+y\sin\varphi_i)(R-r)\\ +2L(R-r-x\cos\varphi_i-y\sin\varphi_i)\cos \theta_i +2Lz\sin\theta_i
l2=(R−r)2+L2+x2+y2+z2−2(xcosφi+ysinφi)(R−r)+2L(R−r−xcosφi−ysinφi)cosθi+2Lzsinθi
由于
R
,
r
,
l
,
L
,
φ
i
,
x
,
y
,
z
R,r,l,L,\varphi_i,x,y,z
R,r,l,L,φi,x,y,z均为已知量,故使得
A
=
2
L
(
R
−
r
−
x
cos
φ
i
−
y
sin
φ
i
)
B
=
2
L
z
C
=
(
R
−
r
)
2
+
L
2
+
x
2
+
y
2
+
z
2
−
2
(
x
cos
φ
i
+
y
sin
φ
i
)
(
R
−
r
)
−
l
2
\begin{align*} &A = 2L(R-r-x\cos\varphi_i-y\sin\varphi_i)\\ &B = 2Lz\\ &C = (R-r)^2+L^2+x^2+y^2+z^2-2(x\cos\varphi_i+y\sin\varphi_i)(R-r)-l^2\\ \end{align*}
A=2L(R−r−xcosφi−ysinφi)B=2LzC=(R−r)2+L2+x2+y2+z2−2(xcosφi+ysinφi)(R−r)−l2
那么原式就可以化成,
A
cos
θ
i
+
B
sin
θ
i
+
C
=
0
A\cos\theta_i+B\sin\theta_i+C = 0
Acosθi+Bsinθi+C=0
其中A,B,C均是常数。再带入万能公式,
sin
θ
i
=
2
t
1
+
t
2
,
cos
θ
i
=
1
−
t
2
1
+
t
2
,
其中
t
=
tan
θ
i
2
\begin{equation*} \sin \theta_i=\frac{2 t}{1+t^2}, \cos \theta_i=\frac{1-t^2}{1+t^2}, 其中t= \tan\frac{\theta_i}{2} \end{equation*}
sinθi=1+t22t,cosθi=1+t21−t2,其中t=tan2θi
得到,
(
C
−
A
)
t
2
+
2
B
t
+
(
A
+
C
)
=
0
(C-A)t^2+2Bt+(A+C)= 0
(C−A)t2+2Bt+(A+C)=0
解得方程为,
t
=
−
2
B
−
4
B
2
−
4
(
C
2
−
A
2
)
2
(
C
−
A
)
=
−
B
−
A
2
+
B
2
−
C
2
C
−
A
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}
t=2(C−A)−2B−4B2−4(C2−A2)=C−A−B−A2+B2−C2
其中,
{
A
=
2
L
(
R
−
r
−
x
cos
φ
i
−
y
sin
φ
i
)
B
=
2
L
z
C
=
(
R
−
r
)
2
+
L
2
+
x
2
+
y
2
+
z
2
−
2
(
x
cos
φ
i
+
y
sin
φ
i
)
(
R
−
r
)
−
l
2
\begin{cases} &A = 2L(R-r-x\cos\varphi_i-y\sin\varphi_i)\\ &B = 2Lz\\ &C = (R-r)^2+L^2+x^2+y^2+z^2-2(x\cos\varphi_i+y\sin\varphi_i)(R-r)-l^2\\ \end{cases}
⎩
⎨
⎧A=2L(R−r−xcosφi−ysinφi)B=2LzC=(R−r)2+L2+x2+y2+z2−2(xcosφi+ysinφi)(R−r)−l2
具体的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