写在前面
学习代码都记录在个人github上,欢迎关注~
Matlab机器人工具箱版本9.10
机械臂用的是五自由度的,我测试时发现逆解精度存在一些问题,目前还没找到是求解析解时出错还是编程过程有问题,还是算法本身考虑不周到,欢迎有研究过的大神们批评指正!感谢~
常规插补算法
假设机器人末端由P1点沿直线运动到P2点, ( x 1 , y 1 , z 1 ) \left(x_{1}, y_{1}, z_{1}\right) (x1,y1,z1)和 ( α ( 1 , β 1 , γ 1 ) \left(\alpha(_{1}, \beta_{1}, \gamma_{1}\right) (α(1,β1,γ1)分别为起点P1的位置和RPY角, ( x 2 , y 2 , z 2 ) \left(x_{2}, y_{2}, z_{2}\right) (x2,y2,z2)和 ( α 2 , β 2 , γ 2 ) \left(\alpha_{2}, \beta_{2}, \gamma_{2}\right) (α2,β2,γ2)分别为终点P2的位置和RPY角, ( x i , y i , z i ) \left(x_{i}, y_{i}, z_{i}\right) (xi,yi,zi)和 ( α i , β i , γ i ) \left(\alpha_{i}, \beta_{i}, \gamma_{i}\right) (αi,βi,γi)分别为中间插补点Pi的位置和RPY角。
各个插值点位置坐标向量和RPY角度向量可以表示为:
{
x
=
x
1
+
λ
Δ
x
y
=
y
1
+
λ
Δ
y
z
=
z
1
+
λ
Δ
z
(
1
)
\left\{\begin{array}{l}{x=x_{1}+\lambda \Delta x} \\ {y=y_{1}+\lambda \Delta y} \\ {z=z_{1}+\lambda \Delta z}\end{array}\right.(1)
⎩⎨⎧x=x1+λΔxy=y1+λΔyz=z1+λΔz(1)
{ α = α 1 + λ Δ α β = β 1 + λ Δ β γ = γ 1 + λ Δ γ ( 2 ) \left\{\begin{array}{l}{\alpha=\alpha_{1}+\lambda \Delta \alpha} \\ {\beta=\beta_{1}+\lambda \Delta \beta} \\ {\gamma=\gamma_{1}+\lambda \Delta \gamma}\end{array}\right.(2) ⎩⎨⎧α=α1+λΔαβ=β1+λΔβγ=γ1+λΔγ(2)
其中,
(
x
,
y
,
z
)
(x, y, z)
(x,y,z)和
(
α
,
β
,
γ
)
(\alpha, \beta, \gamma)
(α,β,γ)表示插值点的位置坐标向量和RPY角度向量,
λ
\lambda
λ为归一化因子,采用抛物线过渡的线性函数(即梯形加减速)以保证整段轨迹上的位移和速度都连续。
(
Δ
x
,
Δ
y
,
Δ
z
)
(\Delta x, \Delta y, \Delta z)
(Δx,Δy,Δz)和
(
Δ
α
,
Δ
β
,
Δ
γ
)
(\Delta \alpha, \Delta \beta, \Delta \gamma)
(Δα,Δβ,Δγ)分别表示首末位置间的位置和RPY角度的增量,其求解为:
{
Δ
x
=
x
2
−
x
1
Δ
y
=
y
2
−
y
1
Δ
z
=
z
2
−
z
1
Δ
α
=
α
2
−
α
1
Δ
β
=
β
2
−
β
1
Δ
γ
=
γ
2
−
γ
1
(
3
)
\left\{\begin{array}{l}{\Delta x=x_{2}-x_{1}} \\ {\Delta y=y_{2}-y_{1}} \\ {\Delta z=z_{2}-z_{1}} \\ {\Delta \alpha=\alpha_{2}-\alpha_{1}} \\ {\Delta \beta=\beta_{2}-\beta_{1}} \\ {\Delta \gamma=\gamma_{2}-\gamma_{1}}\end{array}\right.(3)
⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧Δx=x2−x1Δy=y2−y1Δz=z2−z1Δα=α2−α1Δβ=β2−β1Δγ=γ2−γ1(3)
所以问题被转变为对归一化因子
λ
\lambda
λ的求解,求解方法后面会讲。下面先了解记录两种常见的插补算法:直线插补和空间圆弧插补。
直线插补算法
一般最简单的做法为,已知空间直线的起点S位置坐标为
(
x
1
,
y
1
,
z
1
)
(x_{1},y_{1}, z_{1})
(x1,y1,z1),终点D位置坐标为
(
x
2
,
y
2
,
z
2
)
(x_{2}, y_{2}, z_{2})
(x2,y2,z2)和插补次数N,则有
{
Δ
x
=
(
x
2
−
x
1
)
/
(
N
+
1
)
Δ
y
=
(
y
2
−
y
1
)
/
(
N
+
1
)
Δ
z
=
(
z
2
−
z
1
)
/
(
N
+
1
)
(
4
)
\left\{\begin{array}{l}{\Delta x=\left(x_{2}-x_{1}\right) /(N+1)} \\ {\Delta y=\left(y_{2}-y_{1}\right) /(N+1)} \\ {\Delta z=\left(z_{2}-z_{1}\right) /(N+1)}\end{array}\right.(4)
⎩⎨⎧Δx=(x2−x1)/(N+1)Δy=(y2−y1)/(N+1)Δz=(z2−z1)/(N+1)(4)
对于该直线上任一点i(1<= i <= N)有
{
x
i
=
x
1
+
Δ
x
⋅
i
y
i
=
y
1
+
Δ
y
⋅
i
z
i
=
z
1
+
Δ
z
⋅
i
(
5
)
\left\{\begin{array}{l}{x_{i}=x_{1}+\Delta x \cdot i} \\ {y_{i}=y_{1}+\Delta y \cdot i} \\ {z_{i}=z_{1}+\Delta z \cdot i}\end{array}\right.(5)
⎩⎨⎧xi=x1+Δx⋅iyi=y1+Δy⋅izi=z1+Δz⋅i(5)
这种方法很简单,对于机械臂的运动而言只需要加上RPY角的变化即可,此处不详述。
基于抛物线过渡(梯形加减速)的空间直线插补的核心在于归一化参数 λ \lambda λ的计算,同时加入速度规划的物理条件和约束。因此得到的直线插补算法需要初始化的参数有机械臂末端线速度 v s v_{s} vs、加速度 a a a_{a} aa、减速度 a d a_{d} ad,需要计算得到的插值参数有总位移S和插值点数N。
对于空间直线插补而言,总位移计算公式如下:
S
e
=
(
x
2
−
x
1
)
2
+
(
y
2
−
y
1
)
2
+
(
z
2
−
z
1
)
2
(
6
)
S_{e}=\sqrt{\left(x_{2}-x_{1}\right)^{2}+\left(y_{2}-y_{1}\right)^{2}+\left(z_{2}-z_{1}\right)^{2}}(6)
Se=(x2−x1)2+(y2−y1)2+(z2−z1)2(6)
插值点数的计算公式如下:
N
=
P
n
S
e
V
s
(
7
)
\mathrm{N}=P_{n} \frac{S_{e}}{V_{s}}(7)
N=PnVsSe(7)
其中,
P
n
P_{n}
Pn为插值参数,可以根据情况进行调整。从公式(7)可以看出,插值点数N和总位移
S
e
S_{e}
Se成正比,与机械臂末端的线速度
v
s
v_{s}
vs成反比。也就是说,如果总位移越大,线速度越小,则插值点数应该越多。原因在于,如果位移越大,在速度不变的情况下,一定要保证更多的插值点才能对规划曲线进行有效的拟合。同样的,如果位移不变,为了得到大的运动速度,则要降低插值点数,因为插值过程是一个等时间间隔的过程,插值点数越多,意味着所需时间越长。
空间圆弧插补算法
1.圆心和半径
如图所示,{O-XYZ}为机器人基座坐标系。P1、P2、P3为空间中任意不共线的三个点,其中P1
(
x
1
,
y
1
,
z
1
)
(x_{1}, y_{1}, z_{1})
(x1,y1,z1)、P2
(
x
2
,
y
2
,
z
3
)
(x_{2}, y_{2}, z_{3})
(x2,y2,z3)、P3
(
x
3
,
y
3
,
z
3
)
(x_{3}, y_{3}, z_{3})
(x3,y3,z3)为三点在基座标系中的坐标。那么这三个点可以唯一确定一个外接圆,该园所在平面
M
1
M_{1}
M1的方程如下:
∣
x
−
x
3
y
−
y
3
z
−
z
3
x
1
−
x
3
y
1
−
y
3
z
1
−
z
3
x
2
−
x
3
y
2
−
y
3
z
2
−
z
3
∣
=
0
(
8
)
\left|\begin{array}{ccc}{x-x_{3}} & {y-y_{3}} & {z-z_{3}} \\ {x_{1}-x_{3}} & {y_{1}-y_{3}} & {z_{1}-z_{3}} \\ {x_{2}-x_{3}} & {y_{2}-y_{3}} & {z_{2}-z_{3}}\end{array}\right|=0 (8)
∣∣∣∣∣∣x−x3x1−x3x2−x3y−y3y1−y3y2−y3z−z3z1−z3z2−z3∣∣∣∣∣∣=0(8)
该外接圆方程的一般形式如下:
A
1
x
+
B
1
y
+
C
1
z
+
D
1
=
0
(
9
)
A_{1} x+B_{1} y+C_{1} z+D_{1}=0(9)
A1x+B1y+C1z+D1=0(9)
其中,
A 1 = ( y 1 − y 3 ) ( z 2 − z 3 ) − ( y 2 − y 3 ) ( z 1 − z 3 ) A_{1} = (y_{1} - y_{3})(z_{2} - z_{3}) - (y_{2} - y_{3})(z_{1} - z_{3}) A1=(y1−y3)(z2−z3)−(y2−y3)(z1−z3)
B 1 = ( x 2 − x 3 ) ( z 1 − z 3 ) − ( x 1 − x 3 ) ( z 2 − z 3 ) B_{1} = (x_{2}-x_{3})(z_{1} - z_{3}) - (x_{1} - x_{3})(z_{2} - z_{3}) B1=(x2−x3)(z1−z3)−(x1−x3)(z2−z3)
C 1 = ( x 1 − x 3 ) ( y 2 − y 3 ) − ( x 2 − x 3 ) ( y 1 − y 3 ) C_{1} = (x_{1} - x_{3})(y_{2} - y_{3}) - (x_{2} - x_{3})(y_{1} - y_{3}) C1=(x1−x3)(y2−y3)−(x2−x3)(y1−y3)
D 1 = − ( A 1 ⋅ x 3 + B 1 ⋅ y 3 + C 1 ⋅ z 3 ) D_{1} = -(A_{1}\cdot x_{3} + B_{1} \cdot y_{3} + C_{1} \cdot z_{3}) D1=−(A1⋅x3+B1⋅y3+C1⋅z3)
P1P2的垂直平分面M2的方程如下:
A
2
x
+
B
2
y
+
C
2
z
+
D
2
=
0
(
10
)
A_{2} x+B_{2} y+C_{2} z+D_{2}=0(10)
A2x+B2y+C2z+D2=0(10)
其中,
A 2 = x 2 − x 1 A_{2} = x_{2} - x_{1} A2=x2−x1
B 2 = y 2 − y 1 B_{2} = y_{2} - y_{1} B2=y2−y1
C 2 = z 2 − z 1 C_{2} = z_{2} - z_{1} C2=z2−z1
D 2 = [ ( x 2 2 − x 1 2 ) + ( y 2 2 − y 1 2 ) + ( z 2 2 − z 1 2 ) ] / 2 D_{2} = \left[\left(x_{2}^{2}-x_{1}^{2}\right)+\left(y_{2}^{2}-y_{1}^{2}\right)+\left(z_{2}^{2}-z_{1}^{2}\right)\right] /2 D2=[(x22−x12)+(y22−y12)+(z22−z12)]/2
P2P3的垂直平分面M3的方程如下:
A
3
x
+
B
3
y
+
C
3
z
+
D
3
=
0
(
11
)
A_{3} x+B_{3} y+C_{3} z+D_{3}=0(11)
A3x+B3y+C3z+D3=0(11)
其中,
A 3 = x 3 − x 2 A_{3} = x_{3} - x_{2} A3=x3−x2
B 3 = y 3 − y 2 B_{3} = y_{3} - y_{2} B3=y3−y2
C 3 = z 3 − z 2 C_{3} = z_{3} - z_{2} C3=z3−z2
D 3 = − [ ( x 3 2 − x 2 2 ) + ( y 3 2 − y 2 2 ) + ( z 3 2 − z 2 2 ) ] / 2 D_{3} = -\left[\left(x_{3}^{2}-x_{2}^{2}\right)+\left(y_{3}^{2}-y_{2}^{2}\right)+\left(z_{3}^{2}-z_{2}^{2}\right)\right] /2 D3=−[(x32−x22)+(y32−y22)+(z32−z22)]/2
综上,联立M1、M2、M3三个平面的方程,可得到如下算式:
∣
A
1
B
1
C
1
A
2
B
2
C
2
A
3
B
3
C
3
∣
∣
x
y
z
∣
=
∣
−
D
1
−
D
2
−
D
3
∣
(
12
)
\left|\begin{array}{lll}{A_{1}} & {B_{1}} & {C_{1}} \\ {A_{2}} & {B_{2}} & {C_{2}} \\ {A_{3}} & {B_{3}} & {C_{3}}\end{array}\right|\left|\begin{array}{l}{x} \\ {y} \\ {z}\end{array}\right|=\left|\begin{array}{c}{-D_{1}} \\ {-D_{2}} \\ {-D_{3}}\end{array}\right|(12)
∣∣∣∣∣∣A1A2A3B1B2B3C1C2C3∣∣∣∣∣∣∣∣∣∣∣∣xyz∣∣∣∣∣∣=∣∣∣∣∣∣−D1−D2−D3∣∣∣∣∣∣(12)
公式(12)有且仅有唯一一组解,可解得三点外接圆圆心P0的位置坐标为
(
x
0
,
y
0
,
z
0
)
(x_{0}, y_{0}, z_{0})
(x0,y0,z0),根据圆心坐标可求得外接圆半径为
r
=
(
x
1
−
x
0
)
2
+
(
y
1
−
y
0
)
2
+
(
z
1
−
z
0
)
2
(
13
)
r=\sqrt{\left(x_{1}-x_{0}\right)^{2}+\left(y_{1}-y_{0}\right)^{2}+\left(z_{1}-z_{0}\right)^{2}}(13)
r=(x1−x0)2+(y1−y0)2+(z1−z0)2(13)
2.坐标变换
在以圆心P0为原点的基础上建立新的坐标系{P0-X0Y0Z0}。新坐标系中的Z0为平面M1的法矢量,从上文可得Z0轴的方向余弦为:
a
x
=
A
1
/
L
,
a
y
=
B
1
/
L
,
a
z
=
C
1
/
L
(
14
)
a_{x}=A_{1} / L, \quad a_{y}=B_{1} / L, \quad a_{z}=C_{1} / L(14)
ax=A1/L,ay=B1/L,az=C1/L(14)
其中,
L
=
(
A
1
2
+
B
1
2
+
C
1
2
)
1
/
2
L=\left(A_{1}^{2}+B_{1}^{2}+C_{1}^{2}\right)^{1 / 2}
L=(A12+B12+C12)1/2
此处,规定新坐标系{P0-X0Y0Z0}的X0为P0P1的矢量方向,故可得X0轴的方向余弦如下:
n
i
=
(
i
1
−
i
0
)
(
x
1
−
x
0
)
2
+
(
y
1
−
y
0
)
2
+
(
z
1
−
z
0
)
2
(
15
)
n_{i}=\frac{\left(i_{1}-i_{0}\right)}{\sqrt{\left(x_{1}-x_{0}\right)^{2}+\left(y_{1}-y_{0}\right)^{2}+\left(z_{1}-z_{0}\right)^{2}}}(15)
ni=(x1−x0)2+(y1−y0)2+(z1−z0)2(i1−i0)(15)
其中,i= x, y, z。
确定Z0和X0的方向后,根据右手定则即可确定Y0的方向,如下:
Y
1
=
X
1
×
Z
1
(
16
)
\boldsymbol{Y}_{1}=\boldsymbol{X}_{1} \times \boldsymbol{Z}_{1}(16)
Y1=X1×Z1(16)
在得到新坐标系三个轴的方向余弦后,根据旋转矩阵的定义,可得到新坐标系{P0-X0Y0Z0}相对于基座标系{O-XYZ}的齐次变换矩阵
T
=
[
X
1
Y
1
Z
1
P
1
0
0
0
1
]
(
17
)
T=\left[\begin{array}{cccc}{X_{1}} & {Y_{1}} & {Z_{1}} & {P_{1}} \\ {0} & {0} & {0} & {1}\end{array}\right](17)
T=[X10Y10Z10P11](17)
因此,可通过齐次变换矩阵T将基座标系内的空间圆弧变换到新坐标系内的平面圆弧,简化求解过程。
3.转换为平面圆弧问题
根据齐次变换矩阵,P1、P2、P3在新坐标系下的坐标Q1、Q2、Q3可通过下面公式计算得到:
∣
Q
i
,
1
∣
T
=
T
−
1
∣
P
i
,
1
∣
T
(
i
=
1
,
2
,
3
)
(
18
)
\left|Q_{i}, 1\right|^{\mathrm{T}}=T^{-1}\left|P_{i}, 1\right|^{\mathrm{T}}(i=1,2,3)(18)
∣Qi,1∣T=T−1∣Pi,1∣T(i=1,2,3)(18)
在平面内进行圆弧插补,实际上就是对圆心角进行插补,因此核心就是求出圆弧的圆心角,同时还需要注意顺逆时针的问题。
4.圆心角和插值点数N
由于atan2函数返回的是原点至点(x,y)的方位角,即与 x 轴的夹角。也可以理解为复数 x+yi 的辐角。返回值的单位为弧度。在数学坐标系中,结果为正表示从 X 轴逆时针旋转的角度,结果为负表示从 X 轴顺时针旋转的角度。此时为了判断圆弧是顺时针还是逆时针,需要将角度值转换成0~2 π \pi π的范围。
假设
∠
P
1
P
0
P
2
\angle P_{1} P_{0} P_{2}
∠P1P0P2为
P
0
P
2
P_{0}P_{2}
P0P2与
P
0
X
0
P_{0}X_{0}
P0X0正向的夹角,有
∠
P
1
P
0
P
2
=
atan
2
(
P
2
y
,
P
2
x
)
+
n
∗
2
π
(
19
)
\angle P_{1} P_{0} P_{2}=\operatorname{atan} 2\left(P_{2 y}, P_{2 x}\right)+n * 2 \pi(19)
∠P1P0P2=atan2(P2y,P2x)+n∗2π(19)
其中
P
2
y
、
P
2
x
P_{2y}、P_{2x}
P2y、P2x是P2在新坐标系下的坐标值。此时存在如下两种情况:
- 当 atan 2 ( P 2 y , P 2 x ) < 0 \operatorname{atan} 2\left(P_{2 y}, P_{2 x}\right)<0 atan2(P2y,P2x)<0时, n = 1 n=1 n=1;
- 否则, n = 0 n=0 n=0。
同理,假设
∠
P
1
P
0
P
3
\angle P_{1} P_{0}P_{3}
∠P1P0P3为
P
0
P
3
P_{0}P_{3}
P0P3与
P
0
X
0
P_{0}X_{0}
P0X0正向的夹角,有
∠
P
1
P
0
P
3
=
atan
2
(
P
3
y
,
P
3
x
)
+
n
∗
2
π
(
20
)
\angle P_{1} P_{0} P_{3}=\operatorname{atan} 2\left(P_{3 y}, P_{3 x}\right)+n * 2 \pi(20)
∠P1P0P3=atan2(P3y,P3x)+n∗2π(20)
其中
P
3
y
、
P
3
x
P_{3y}、P_{3x}
P3y、P3x是P2在新坐标系下的坐标值。此时存在如下两种情况:
- 当 atan 2 ( P 3 y , P 3 x ) < 0 \operatorname{atan} 2\left(P_{3 y}, P_{3 x}\right)<0 atan2(P3y,P3x)<0时, n = 1 n=1 n=1;
- 否则, n = 0 n=0 n=0。
通过比较 ∠ P 1 P 0 P 2 \angle P_{1} P_{0} P_{2} ∠P1P0P2和 ∠ P 1 P 0 P 3 \angle P_{1} P_{0} P_{3} ∠P1P0P3的大小可以确定圆弧的方向,同时可以求出圆弧圆心角 Ω \Omega Ω:
- 当 ∠ P 1 P 0 P 2 < ∠ P 1 P 0 P 3 \angle P_{1} P_{0} P_{2}<\angle P_{1} P_{0} P_{3} ∠P1P0P2<∠P1P0P3时,圆弧为逆时针,圆心角 Ω = ∠ P 1 P 0 P 3 \Omega=\angle P_{1} P_{0} P_{3} Ω=∠P1P0P3;
- 当 ∠ P 1 P 0 P 2 ≥ ∠ P 1 P 0 P 3 \angle P_{1} P_{0} P_{2} \geq \angle P_{1} P_{0} P_{3} ∠P1P0P2≥∠P1P0P3时, Ω = 2 π − ∠ P 1 P 0 P 3 \Omega=2\pi - \angle P_{1} P_{0} P_{3} Ω=2π−∠P1P0P3;
对应基于抛物线过渡(梯形加减速)的空间圆弧插补而言,需要初始化的运动参数有机械臂末端角速度
ω
s
\omega_{s}
ωs和角加速度a,需要计算的插值参数为圆心角
Ω
\Omega
Ω和插值点数N。上文已经求得圆心角,插值点数计算公式如下:
N
=
P
n
Ω
ω
s
(
21
)
\mathrm{N}=P_{n} \frac{\Omega}{\omega_{s}}(21)
N=PnωsΩ(21)
归一化因子 λ \lambda λ的求解
预备知识
机器人学回炉重造(6):关节空间规划方法——梯形加减速(与抛物线拟合的线性函数)、S型曲线规划
设机械臂在匀速运动时的线速度为
v
s
v_{s}
vs,抛物线段的加减速度均为
a
a
a,可以计算出加速阶段和减速阶段的时间和位移为
T
1
=
V
s
a
(
22
)
T_{1}=\frac{V_{s}}{a}(22)
T1=aVs(22)
S 1 = 1 2 a T 1 2 ( 23 ) S_{1}=\frac{1}{2} a T_{1}^{2}(23) S1=21aT12(23)
设机械臂末端运动的总位移为
S
e
S_{e}
Se,总时间为:
T
e
=
2
T
1
+
s
e
−
2
S
1
V
s
(
24
)
T_{e}=2 T_{1}+\frac{s_{e}-2 S_{1}}{V_{s}}(24)
Te=2T1+Vsse−2S1(24)
将位移、速度和加速度进行归一化处理得到上述计算量的归一化参数:
S
1
λ
=
s
1
s
e
(
25
)
S_{1 \lambda}=\frac{s_{1}}{s_{e}}(25)
S1λ=ses1(25)
T 1 λ = T 1 T e ( 26 ) T_{1 \lambda}=\frac{T_{1}}{T_{e}}(26) T1λ=TeT1(26)
T 2 λ = 1 − T 1 λ ( 27 ) T_{2 \lambda}=1-T_{1 \lambda}(27) T2λ=1−T1λ(27)
a λ = 2 s 1 λ T 1 λ 2 ( 28 ) a_{\lambda}=\frac{2 s_{1 \lambda}}{T_{1 \lambda}^{2}}(28) aλ=T1λ22s1λ(28)
结合梯形加减速的位移公式,可以推导出归一化因子
λ
\lambda
λ的计算公式如下:
λ
=
{
1
2
a
λ
t
2
(
0
≤
t
≤
T
1
λ
)
1
2
a
λ
T
1
λ
2
+
a
λ
T
1
λ
(
t
−
T
1
λ
)
(
T
1
λ
<
t
≤
T
2
λ
)
1
2
a
λ
T
1
λ
2
+
a
λ
T
1
λ
(
t
−
T
1
λ
)
−
1
2
a
λ
(
t
−
T
2
λ
)
2
(
T
2
λ
<
t
≤
1
)
(
29
)
\lambda=\left\{\begin{array}{cc}{\frac{1}{2} a_{\lambda} t^{2}} & {\left(0 \leq t \leq T_{1 \lambda}\right)} \\ {\frac{1}{2} a_{\lambda} T_{1 \lambda}^{2}+a_{\lambda} T_{1 \lambda}\left(t-T_{1 \lambda}\right)} & {\left(T_{1 \lambda}<t \leq T_{2 \lambda}\right)} \\ {\frac{1}{2} a_{\lambda} T_{1 \lambda}^{2}+a_{\lambda} T_{1 \lambda}\left(t-T_{1 \lambda}\right)-\frac{1}{2} a_{\lambda}\left(t-T_{2 \lambda}\right)^{2}} & {\left(T_{2 \lambda}<t \leq 1\right)}\end{array}\right.(29)
λ=⎩⎨⎧21aλt221aλT1λ2+aλT1λ(t−T1λ)21aλT1λ2+aλT1λ(t−T1λ)−21aλ(t−T2λ)2(0≤t≤T1λ)(T1λ<t≤T2λ)(T2λ<t≤1)(29)
式中,t表示时间,
t
=
i
/
N
(
0
<
=
t
<
=
1
)
t=i/N(0<=t<=1)
t=i/N(0<=t<=1),其中N表示总的插值点数,i = 1、2、3、……、N-1、N。每个插值点的时间值,都有一个
λ
\lambda
λ与之对应,结合公式(1)(2)(3)可以得到各个插值点位置坐标向量和RPY角度向量,最后就是解逆运动学方程,得到每个插值点对应的关节变量,生成轨迹。
Matlab实现代码
预备知识:
模型是我买的五自由度机械臂,之前已经推导过逆运动学的解析解,这里直接拿来用。
先是预备程序:
- 高斯列主元消去法,解圆心方程;
- 五自由度逆运动学解析解推导(前面博客有);
- 归一化因子推导。
%% 高斯列主元消去法
function x = Gauss_lie(n, A, b)
for k = 1: n-1
a_max = abs(A(k, k));
cnt = k;
for i = k: n
if (abs(A(i, k)) > a_max)
a_max = abs(A(i, k));
cnt = i; % 确定下标i
end
end
if (A(cnt, k) == 0)
fprintf('Gauss_lie: no unique solution\n');
return;
end
% 换行
if (cnt ~= k)
t = 0; s = 0;
for j = k: n
t = A(k, j);
A(k, j) = A(cnt, j);
A(cnt, j) = t;
s = b(cnt);
b(cnt) = b(k);
b(k) = s;
end
end
% step 5
for i = k+1: n
L(i) = A(i, k) / A(k, k);
for j = k+1: n
A(i, j) = A(i, j) - L(i)*A(k, j);
end
b(i) = b(i) - L(i)*b(k);
end
end
for i = 1: n
if (A(i, i) == 0)
fprintf('Gauss_lie no unique solution\n');
return;
end
end
% 回代
x(n) = b(n) / A(n, n);
for i = n-1: -1: 1
sum_a = 0;
for j = i+1: n
sum_a = sum_a + A(i, j)*x(j);
end
x(i) = (b(i) - sum_a) / A(i, i);
end
end
% 归一化处理
% 梯形加减速曲线
% 输入参数:机械臂末端运动总位移(角度)pos
% 机械臂末端线速度(角速度)vel
% 加速度减速度accl(设定加减速段accl相同)
% 插值点数N
function lambda = Normalization(pos, vel, accl, N)
% 加减速段的时间和位移
T1 = vel / accl;
S1 = (1/2) * accl * T1^2;
% 总时间
Te = 2*T1 + (pos - 2*S1) / vel;
% 归一化处理
S1_ = S1 / pos;
T1_ = T1 / Te;
T2_ = 1 - T1_;
accl_ = 2*S1_ / T1_^2;
% lambda求解公式
for i = 0: N
t = i / N;
if (t >= 0 && t <= T1_)
lambda(i+1) = (1/2) * accl_ * t^2;
elseif (t > T1_ && t <= T2_)
lambda(i+1) = (1/2)*accl_*T1_^2 + accl_*T1_*(t - T1_);
elseif (t > T2_ && t <= 1)
lambda(i+1) = (1/2)*accl_*T1_^2 + accl_*T1_*(T2_ - T1_) + (1/2)*accl_*power(t - T2_, 2);
end
end
end
核心程序:
% 空间直线位置插补与RPY角姿态插补 + 梯形加减速归一化处理
% 参数:起点S位置, 终点D位置, 末端线速度vs, 加减速度a
% 起点S的RPY角、终点D的RPY角
% 返回值:插值点(不包括起点S和终点D)
function [x y z alp beta gama N] = SpaceLine(S, D, S_, D_, vs, a)
x1 = S(1); y1 = S(2); z1 = S(3);
x2 = D(1); y2 = D(2); z2 = D(3);
alp1 = S_(1); beta1 = S_(2); gama1 = S_(3);
alp2 = D_(1); beta2 = D_(2); gama2 = D_(3);
P = 1; % 插值参数,增加插值点数,避免过小
% 总位移S
s = sqrt(power(x2 - x1, 2) + power(y2 - y1, 2) + power(z2 - z1, 2))
% 插值点数N
N = ceil(P*s / vs)
% 求归一化参数
% function lambda = Normalization(pos, vel, accl, N)
lambda = Normalization(s, vs, a, N)
delta_x = x2 - x1;
delta_y = y2 - y1;
delta_z = z2 - z1;
delta_alp = alp2 - alp1;
delta_beta = beta2 - beta1;
delta_gama = gama2 - gama1;
for i = 1: N+1
x(i) = x1 + delta_x*lambda(i);
y(i) = y1 + delta_y*lambda(i);
z(i) = z1 + delta_z*lambda(i);
alp(i) = alp1 + delta_alp*lambda(i);
beta(i) = beta1 + delta_beta*lambda(i);
gama(i) = gama1 + delta_gama*lambda(i);
end
end
% 空间圆弧位置插补与RPY角姿态插补 + 梯形加减速归一化处理
% 参数: 起点S位置和RPY角, 中间点M位置, 终点D位置和RPY角,末端角速度,角加减速度
% 方便起见,角速度和角加速度均为角度制
function [x y z alp beta gama N] = SpaceCircle(S, M, D, S_, D_, ws, a)
x1 = S(1); x2 = M(1); x3 = D(1);
y1 = S(2); y2 = M(2); y3 = D(2);
z1 = S(3); z2 = M(3); z3 = D(3);
alp1 = S_(1); beta1 = S_(2); gama1 = S_(3);
alp2 = D_(1); beta2 = D_(2); gama2 = D_(3);
A1 = (y1 - y3)*(z2 - z3) - (y2 - y3)*(z1 - z3);
B1 = (x2 - x3)*(z1 - z3) - (x1 - x3)*(z2 - z3);
C1 = (x1 - x3)*(y2 - y3) - (x2 - x3)*(y1 - y3);
D1 = -(A1*x3 + B1*y3 + C1*z3);
A2 = x2 - x1;
B2 = y2 - y1;
C2 = z2 - z1;
D2 = -((x2^2 - x1^2) + (y2^2 - y1^2) + (z2^2 - z1^2)) / 2;
A3 = x3 - x2;
B3 = y3 - y2;
C3 = z3 - z2;
D3 = -((x3^2 - x2^2) + (y3^2 - y2^2) + (z3^2 - z2^2)) / 2;
A = [A1, B1, C1; A2, B2, C2; A3, B3, C3]
b = [-D1, -D2, -D3]'
% 圆心
C = Gauss_lie(3, A, b)
x0 = C(1); y0 = C(2); z0 = C(3);
plot3(x0, y0, z0, 'bo')
hold on
% 外接圆半径
r = sqrt(power(x1 - x0, 2) + power(y1 - y0, 2) + power(z1 - z0, 2));
% 新坐标系Z0的方向余弦
L = sqrt(A1^2 + B1^2 + C1^2);
ax = A1 / L; ay = B1 / L; az = C1 / L;
% 新坐标系X0的方向余弦
nx = (x1 - x0) / r;
ny = (y1 - y0) / r;
nz = (z1 - z0) / r;
% 新坐标系Y0的方向余弦
o = cross([ax, ay, az], [nx, ny, nz]);
ox = o(1);
oy = o(2);
oz = o(3);
% 相对于基座标系{O-XYZ}, 新坐标系{C-X0Y0Z0}的坐标变换矩阵
T = [nx ox ax x0;
ny oy ay y0;
nz oz az z0;
0 0 0 1]
T_ni = T^-1
% 求在新坐标系{C-X0Y0Z0}下S、M和D的坐标
S_ = (T^-1)*[S'; 1]
M_ = (T^-1)*[M'; 1]
D_ = (T^-1)*[D'; 1]
x1_ = S_(1) , y1_ = S_(2), z1_ = S_(3)
x2_ = M_(1), y2_ = M_(2), z2_ = M_(3)
x3_ = D_(1), y3_ = D_(2), z3_ = D_(3)
% 判断圆弧是顺时针还是逆时针,并求解圆心角
if (atan2(y2_, x2_) < 0)
angle_SOM = atan2(y2_, x2_) + 2*pi;
else
angle_SOM = atan2(y2_, x2_);
end
if (atan2(y3_, x3_) < 0)
angle_SOD = atan2(y3_, x3_) + 2*pi;
else
angle_SOD = atan2(y3_, x3_);
end
% 逆时针
if (angle_SOM < angle_SOD)
flag = 1;
theta = angle_SOD % 圆心角
end
% 顺时针
if (angle_SOM >= angle_SOD)
flag = -1;
theta = 2*pi - angle_SOD % 圆心角
end
% 插补点数N
P = 2; %插补参数,增加插值点数,避免过小
ws = ws*pi / 180; % 角度换成弧度
a = a*pi / 180;
N = P*theta / ws;
% 求归一化参数
lambda = Normalization(theta, ws, a, N);
% 插补原理: 在新平面上进行插补(简化)
% 在新坐标系下z1_,z2_,z3_均为0,即外接圆在新坐标系的XOY平面内
% 此时转化为平面圆弧插补问题
delta_ang = theta;
delta_alp = alp2 - alp1
delta_beta = beta2 - beta1;
delta_gama = gama2 - gama1;
for i = 1: N+1
x_(i) = flag * r * cos(lambda(i)*delta_ang);
y_(i) = flag * r * sin(lambda(i)*delta_ang);
P = T*[x_(i); y_(i); 0; 1];
x(i) = P(1);
y(i) = P(2);
z(i) = P(3);
alp(i) = alp1 + delta_alp*lambda(i);
beta(i) = beta1 + delta_beta*lambda(i);
gama(i) = gama1 + delta_gama*lambda(i);
end
% % figure(1)
% % plot(x_, y_)
% 插补原理: 在原圆弧上进行插补
% 圆弧上任一点处沿前进方向的切向量
% x(1) = x1; y(1) = y1; z(1) = z1;
% for i = 1: N+1
% m(i) = flag*(ay*(z(i) - z0) - az*(y(i) - y0));
% n(i) = flag*(az*(x(i) - x0) - ax*(z(i) - z0));
% l(i) = flag*(ax*(y(i) - y0) - ay*(x(i) - x0));
% delta_s = delta_ang * r;
% E = delta_s / (r*sqrt(ax^2 + ay^2 + az^2));
% G = r / sqrt(r^2 + delta_s^2);
% x(i+1) = x0 + G*(x(i) + E*m(i) - x0);
% y(i+1) = y0 + G*(y(i) + E*n(i) - y0);
% z(i+1) = z0 + G*(z(i) + E*l(i) - z0);
% end
end
测试主程序
% Standard DH
% five_dof robot
% 在关节4和关节5之间增加一个虚拟关节,便于逆运动学计算
clear;
clc;
th(1) = 0; d(1) = 0; a(1) = 0; alp(1) = pi/2;
th(2) = 0; d(2) = 0; a(2) = 1.04;alp(2) = 0;
th(3) = 0; d(3) = 0; a(3) = 0.96; alp(3) = 0;
th(4) = 0; d(4) = 0; a(4) = 0; alp(4) = 0;
th(5) = pi/2; d(5) = 0; a(5) = 0; alp(5) = pi/2;
th(6) = 0; d(6) = 0; a(6) = 0; alp(6) = 0;
th(7) = 0; d(7) = 1.63; a(7) = 0.28; alp(7) = 0;
% DH parameters th d a alpha sigma
L1 = Link([th(1), d(1), a(1), alp(1), 0]);
L2 = Link([th(2), d(2), a(2), alp(2), 0]);
L3 = Link([th(3), d(3), a(3), alp(3), 0]);
L4 = Link([th(4), d(4), a(4), alp(4), 0]);
L5 = Link([th(5), d(5), a(5), alp(5), 0]);
L6 = Link([th(6), d(6), a(6), alp(6), 0]);
L7 = Link([th(7), d(7), a(7), alp(7), 0]);
robot = SerialLink([L1, L2, L3, L4, L5, L6, L7]);
robot.name='MyRobot-5-dof';
robot.display()
% 起点关节角[0 90 30 60 90 0 0]*pi/180
% 终点关节角[0 90 60 60 90 0 0]*pi/180
theta_S = [0 120 100 40 90 0 0]*pi/180;
theta_M = [60 100 100 30 90 0 0]*pi/180;
theta_D = [100 120 100 40 90 0 0]*pi/180;
% robot.teach();
% robot.plot(theta_D);
% hold on
T_S = robot.fkine(theta_S) %起点末端执行器位姿
T_M = robot.fkine(theta_M)
T_D = robot.fkine(theta_D)
% ik_T = five_dof_ikine(T_D)
[S_RPY(1) S_RPY(2) S_RPY(3)] = RPY_angle(T_S); % 起点对应的RPY角
[D_RPY(1) D_RPY(2) D_RPY(3)] = RPY_angle(T_D); % 终点对应的RPY角
S = T_S(1: 3, 4); % 起点对应的位置坐标
M = T_M(1: 3, 4);
D = T_D(1: 3, 4); % 终点对应的位置坐标
vs = 0.06; a = 0.02; % 直线插补速度参数
ws = 5; a = 2.5; % 空间圆弧插补速度参数
% [x y z alp beta gama N] = SpaceLine(S, D, S_RPY, D_RPY, vs, a); % 直线插补,得到插值点(不包括起点和终点)
[x y z alp beta gama N] = SpaceCircle(S', M', D', S_RPY, D_RPY, ws, a);
plot3(x, y, z)
hold on
th1(1) = theta_S(1); th2(1) = theta_S(2); th3(1) = theta_S(3);
th4(1) = theta_S(4); th5(1) = theta_S(5); th6(1) = theta_S(6); th7(1) = theta_S(7);
% t = [0 0 0 0 0];
T = {1, N};
for i = 1: N+1
R = RPY_rot(alp(i), beta(i), gama(i));
R(1, 1:3);
R(2, 1:3);
R(3, 1:3);
T{i} = [R(1, 1:3), x(i);
R(2, 1:3), y(i);
R(3, 1:3), z(i);
0 0 0 1];
theta = five_dof_ikine(T{i});
th = theta(2, 1:5); % 简单取1行逆解
th1(i+1) = th(1);
th2(i+1) = th(2);
th3(i+1) = th(3);
th4(i+1) = th(4);
th5(i+1) = pi/2;
th6(i+1) = th(5);
th7(i+1) = 0;
end
for i = 1: N+1
T_ = robot.fkine([th1(i), th2(i), th3(i), th4(i), th5(i), th6(i), th7(i)]);
traj = T_(1: 3, 4);
a(i) = traj(1);
b(i) = traj(2);
c(i) = traj(3);
plot3(traj(1), traj(2), traj(3), 'r*');
hold on
robot.plot([th1(i), th2(i), th3(i), th4(i), th5(i), th6(i), th7(i)])
end
参考文献
[1]杨淞. 一种六自由度机械臂的运动控制系统设计[D].上海交通大学,2014.
[2]陈国良,黄心汉,王敏.机械手圆周运动的轨迹规划与实现[J].华中科技大学学报(自然科学版),2005(11):69-72.
[3]卓扬娃,白晓灿,陈永明.机器人的三种规则曲线插补算法[J].装备制造技术,2009(11):27-29.
[4]林威,江五讲.工业机器人笛卡尔空间轨迹规划[J].机械工程与自动化,2014(05):141-143.
[5]Chen Z, Wang H, Lu X, et al. Kinematics analysis and application of 5-DOF manipulator with special joint[C]//2017 Chinese Automation Congress (CAC). IEEE, 2017: 7421-7426.