平面2R机器人(二连杆)运动学与动力学建模+附仿真模型

简介: 平面2R机器人(二连杆)运动学与动力学建模+附仿真模型

目录

1 平面2R机器人概述

如图1所示为本文的研究本体——平面2R机器人。对参数进行如下定义:机器人广义坐标 θ 1 \theta _1 θ

1


为连杆1与 y y y轴负半轴的夹角,逆时针为正; θ 2 \theta _2 θ

2


为连杆2与连杆1延长线的夹角,逆时针为正;连杆1、2的关节力矩分别定义为 τ 1 \tau_1 τ

1


、 τ 2 \tau_2 τ

2


。设连杆质量均匀分布,长度都为 l l l,质量都为 M M M,末端执行器笛卡尔坐标为 P ( x , y ) P(x,y) P(x,y)。


image.png

2 运动学建模

2.1 正运动学模型

指数积方法的核心原理是基于运动旋量的指数积公式:

T A T ( θ ) = e [ V 1 ] θ 1 e [ V 2 ] θ 2 ⋯ e [ V n ] θ n T A T ( 0 ) _{\boldsymbol{T}}^{\boldsymbol{A}}\boldsymbol{T}\left( \boldsymbol{\theta } \right) =e^{\left[ \boldsymbol{V}_1 \right] \theta _1}e^{\left[ \boldsymbol{V}_2 \right] \theta _2}\cdots {e^{\left[ \boldsymbol{V}_n \right] \theta _n}}_{\boldsymbol{T}}^{\boldsymbol{A}}\boldsymbol{T}\left( 0 \right)

T

A


T(θ)=e

[V

1


1


e

[V

2


2


⋯e

[V

n


n


 

T

A


T(0)


可以证明,连杆变换之积(DH法)等价于指数积。对平面2R机器人,确定各个关节轴线的线矢量如图2所示,不必建立连杆坐标系。


image.png

图2

各轴线方向如下:

ω 1 = ω 2 = [ 0 0 1 ] , [ ω 1 ] = [ ω 2 ] = [ 0 − 1 0 1 0 0 0 0 0 ] \omega _1=\omega _2=\left[

001

001

\right] ,\mathrm{ }\left[ \omega _1 \right] =\left[ \omega _2 \right] =\left[

010−100000

0−10100000

\right]

ω

1


2


=


 

0

0

1


 


,[ω

1


]=[ω

2


]=


 

0

1

0


 

−1

0

0


 

0

0

0


 



取各个运动轴上的一点:


r 1 = [ 0 0 0 ] , r 2 = [ 0 − l 0 ] r_1=\left[

000

000

\right] ,\mathrm{ }r_2=\left[

0−l0

0−l0

\right]

r

1


=


 

0

0

0


 


,r

2


=


 

0

−l

0


 



可得各个运动旋量的指数形式

e [ V 1 ] θ 1 = [ cos ⁡ θ 1 − sin ⁡ θ 1 0 0 sin ⁡ θ 1 cos ⁡ θ 1 0 0 0 0 1 0 0 0 0 1 ] , e [ V 2 ] θ 2 = [ cos ⁡ θ 2 − sin ⁡ θ 2 0 − l sin ⁡ θ 2 sin ⁡ θ 2 cos ⁡ θ 2 0 − l ( 1 − cos ⁡ θ 2 ) 0 0 1 0 0 0 0 1 ] e^{\left[ \boldsymbol{V}_1 \right] \theta _1}=\left[

cosθ1sinθ100−sinθ1cosθ10000100001

cos⁡θ1−sin⁡θ100sin⁡θ1cos⁡θ10000100001

\right] , e^{\left[ \boldsymbol{V}_2 \right] \theta _2}=\left[

cosθ2sinθ200−sinθ2cosθ2000010−lsinθ2−l(1−cosθ2)01

cos⁡θ2−sin⁡θ20−lsin⁡θ2sin⁡θ2cos⁡θ20−l(1−cos⁡θ2)00100001

\right]

e

[V

1


1


=


 

cosθ

1


sinθ

1


0

0


 

−sinθ

1


cosθ

1


0

0


 

0

0

1

0


 

0

0

0

1


 


,e

[V

2


2


=


 

cosθ

2


sinθ

2


0

0


 

−sinθ

2


cosθ

2


0

0


 

0

0

1

0


 

−lsinθ

2


−l(1−cosθ

2


)

0

1


 



关节变量 θ = [ θ 1 θ 2 ] \boldsymbol{\theta }=\left[

θ1θ2

θ1θ2

\right] θ=[

θ

1



 

θ

2



]为0时即为初始位姿:


T B T ( 0 ) = [ 1 0 0 0 0 1 0 − 2 l 0 0 1 0 0 0 0 1 ] _{T}^{B}\boldsymbol{T}\left( 0 \right) =\left[

1000010000100−2l01

1000010−2l00100001

\right]

T

B


T(0)=


 

1

0

0

0


 

0

1

0

0


 

0

0

1

0


 

0

−2l

0

1


 



所以由 T B T ( θ ) = e [ V 1 ] θ 1 e [ V 2 ] θ 2  ⁣   T S T ( 0 ) _{T}^{B}\boldsymbol{T}\left( \boldsymbol{\theta } \right) =e^{\left[ \boldsymbol{V}_1 \right] \theta _1}e^{\left[ \boldsymbol{V}_2 \right] \theta _2}\!\:_{T}^{S}\boldsymbol{T}\left( 0 \right)

T

B


T(θ)=e

[V

1


1


e

[V

2


2


 

T

S


T(0)即得


T B T ( θ ) = [ cos ⁡ ( θ 1 + θ 2 ) − sin ⁡ ( θ 1 + θ 2 ) 0 l sin ⁡ ( θ 1 + θ 2 ) + l sin ⁡ θ 1 sin ⁡ ( θ 1 + θ 2 ) cos ⁡ ( θ 1 + θ 2 ) 0 − l cos ⁡ ( θ 1 + θ 2 ) − l cos ⁡ θ 1 0 0 1 0 0 0 0 1 ] _{T}^{B}\boldsymbol{T}\left( \boldsymbol{\theta } \right) =\left[

cos(θ1+θ2)sin(θ1+θ2)00−sin(θ1+θ2)cos(θ1+θ2)000010lsin(θ1+θ2)+lsinθ1−lcos(θ1+θ2)−lcosθ101

cos⁡(θ1+θ2)−sin⁡(θ1+θ2)0lsin⁡(θ1+θ2)+lsin⁡θ1sin⁡(θ1+θ2)cos⁡(θ1+θ2)0−lcos⁡(θ1+θ2)−lcos⁡θ100100001

\right]

T

B


T(θ)=


 

cos(θ

1


2


)

sin(θ

1


2


)

0

0


 

−sin(θ

1


2


)

cos(θ

1


2


)

0

0


 

0

0

1

0


 

lsin(θ

1


2


)+lsinθ

1


−lcos(θ

1


2


)−lcosθ

1


0

1


 



可以验证,基于运动旋量法的机器人正运动学模型,与基于D-H法的正运动学模型相同,可交叉验证正运动学模型的理论正确性。

2.2 逆运动学模型

以图3所示的象限为例,以几何方法进行机器人运动学反解。


image.png

对于位形一,在 Δ O A B \varDelta OAB ΔOAB中运用余弦定理可得 2 l 2 cos ⁡ ( π − θ 2 ) = l 2 + l 2 − L 2 2l^2\cos \left( \pi -\theta _2 \right) =l^2+l^2-L^2 2l

2

cos(π−θ

2


)=l

2

+l

2

−L

2

,其中 L 2 = x 2 + y 2 L^2=x^2+y^2 L

2

=x

2

+y

2

,显然末端执行器坐标需要满足 0 < x 2 + y 2 ⩽ 2 l 0<\sqrt{x^2+y^2}\leqslant 2l 0<

x

2

+y

2


⩽2l


解得:

θ 2 = a r c cos ⁡ ( x 2 + y 2 2 l 2 − 1 ) \theta _2=\mathrm{arc}\cos \left( \frac{x^2+y^2}{2l^2}-1 \right)

θ

2


=arccos(

2l

2

x

2

+y

2


−1)


有两个解,位形一对应正解,位形二对应负解。

同样在 Δ O A B \varDelta OAB ΔOAB中运用余弦定理可得

ψ = a r c cos ⁡ ( x 2 + y 2 2 l ) \psi =\mathrm{arc}\cos \left( \frac{\sqrt{x^2+y^2}}{2l} \right)

ψ=arccos(

2l

x

2

+y

2



)


为方便起见,这里总是取 ψ \psi ψ的正解。再根据

β = ∣ a r c tan ⁡ ( y x ) ∣ \beta =\left| \mathrm{arc}\tan \left( \frac{y}{x} \right) \right|

β=


arctan(

x

y


)



可得位形一下的关节参数 θ 1 = π 2 − ( β + ψ ) \theta _1=\frac{\pi}{2}-\left( \beta +\psi \right) θ

1


=

2

π


−(β+ψ)。当机械臂运动到其他象限时同理,求得平面2R机器人运动学反解为:


{ θ 1 = π 2 − ( β ± ψ ) , θ 2 > 0 时取 + θ 1 = π 2 + ( β ± ψ ) , θ 2 > 0 时取 − θ 1 = 3 π 2 − ( β ± ψ ) , θ 2 > 0 时取 + θ 1 = 3 π 2 + ( β ± ψ ) , θ 2 > 0 时取 −

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪θ1=π2−(β±ψ),θ2>0时取+θ1=π2+(β±ψ),θ2>0时取−θ1=3π2−(β±ψ),θ2>0时取+θ1=3π2+(β±ψ),θ2>0时取−

{θ1=π2−(β±ψ),θ2>0时取+θ1=π2+(β±ψ),θ2>0时取−θ1=3π2−(β±ψ),θ2>0时取+θ1=3π2+(β±ψ),θ2>0时取−


 

θ

1


=

2

π


−(β±ψ),θ

2


>0时取+

θ

1


=

2

π


+(β±ψ),θ

2


>0时取−

θ

1


=

2


−(β±ψ),θ

2


>0时取+

θ

1


=

2


+(β±ψ),θ

2


>0时取−

 

2.3 机器人运动学仿真

基于Matlab Robotics工具箱进行平面2R机器人的运动学仿真验证。验证过程如下:分别选取位于四个象限的关节参数

[ θ 1 θ 2 ] = [ 35 ° 55 ° ] , [ θ 1 θ 2 ] = [ 95 ° 25 ° ] , [ θ 1 θ 2 ] = [ 230 ° 20 ° ] , [ θ 1 θ 2 ] = [ 300 ° 70 ° ] \left[

θ1θ2

θ1θ2

\right] =\left[

35\degree55\degree

35\degree55\degree

\right] , \left[

θ1θ2

θ1θ2

\right] =\left[

95\degree25\degree

95\degree25\degree

\right] , \left[

θ1θ2

θ1θ2

\right] =\left[

230\degree20\degree

230\degree20\degree

\right] , \left[

θ1θ2

θ1θ2

\right] =\left[

300\degree70\degree

300\degree70\degree

\right]

[

θ

1


θ

2



]=[

35°

55°


],[

θ

1


θ

2



]=[

95°

25°


],[

θ

1


θ

2



]=[

230°

20°


],[

θ

1


θ

2



]=[

300°

70°


]


进行正运动的验证,而后基于四组末端执行器的坐标进行运动学反解,得到该坐标下的另一个位形对应的关节参数,将该参数代入Matlab机器人模型,验证此时末端执行器的坐标是否与位形一下的重合。将四组参数分别代入上式,得到

[ x y ] = [ 1.57 − 0.82 ] , [ x y ] = [ 1.86 0.59 ] , [ x y ] = [ − 1.71 0.99 ] , [ x y ] = [ − 0.69 − 1.48 ] \left[

xy

xy

\right] =\left[

1.57−0.82

1.57−0.82

\right] , \left[

xy

xy

\right] =\left[

1.860.59

1.860.59

\right] , \left[

xy

xy

\right] =\left[

−1.710.99

−1.710.99

\right] , \left[

xy

xy

\right] =\left[

−0.69−1.48

−0.69−1.48

\right]

[

x

y


]=[

1.57

−0.82


],[

x

y


]=[

1.86

0.59


],[

x

y


]=[

−1.71

0.99


],[

x

y


]=[

−0.69

−1.48


]


将这四组笛卡尔坐标值代入式,得到另一位形的关节参数值


[ θ 1 θ 2 ] = [ 90 ° − 55 ° ] , [ θ 1 θ 2 ] = [ 120 ° − 25 ° ] , [ θ 1 θ 2 ] = [ 249 ° − 20 ° ] , [ θ 1 θ 2 ] = [ 10 ° − 70 ° ] \left[

θ1θ2

θ1θ2

\right] =\left[

90\degree−55\degree

90\degree−55\degree

\right] , \left[

θ1θ2

θ1θ2

\right] =\left[

120\degree−25\degree

120\degree−25\degree

\right] , \left[

θ1θ2

θ1θ2

\right] =\left[

249\degree−20\degree

249\degree−20\degree

\right] , \left[

θ1θ2

θ1θ2

\right] =\left[

10\degree−70\degree

10\degree−70\degree

\right]

[

θ

1


θ

2



]=[

90°

−55°


],[

θ

1


θ

2



]=[

120°

−25°


],[

θ

1


θ

2



]=[

249°

−20°


],[

θ

1


θ

2



]=[

10°

−70°


]


image.png

3 动力学建模

3.1 计算动能

取定连杆上一质量微元 d m \mathrm{d}m dm,其位置坐标为 ( x , y ) (x,y) (x,y),采用极坐标表示即为

连杆I { x = r sin ⁡ θ 1 y = − r cos ⁡ θ 1    连杆II { x = l sin ⁡ θ 1 + r sin ⁡ ( θ 1 + θ 2 ) y = − l cos ⁡ θ 1 − r cos ⁡ ( θ 1 + θ 2 ) \text{连杆I}

{x=rsinθ1y=−rcosθ1

{x=rsin⁡θ1y=−rcos⁡θ1

\,\, \text{连杆II}

{x=lsinθ1+rsin(θ1+θ2)y=−lcosθ1−rcos(θ1+θ2)

{x=lsin⁡θ1+rsin⁡(θ1+θ2)y=−lcos⁡θ1−rcos⁡(θ1+θ2)

连杆I{

x=rsinθ

1


y=−rcosθ

1



连杆II{

x=lsinθ

1


+rsin(θ

1


2


)

y=−lcosθ

1


−rcos(θ

1


2


)



对时间求导,得到

连杆I { x ˙ = r θ ˙ 1 cos ⁡ θ 1 y ˙ = r θ ˙ 1 sin ⁡ θ 1    连杆II { x ˙ = l θ ˙ 1 cos ⁡ θ 1 + r ( θ ˙ 1 + θ ˙ 2 ) cos ⁡ ( θ 1 + θ 2 ) y ˙ = l θ ˙ 1 sin ⁡ θ 1 + r ( θ ˙ 1 + θ ˙ 2 ) sin ⁡ ( θ 1 + θ 2 ) \text{连杆I}

{x˙=rθ˙1cosθ1y˙=rθ˙1sinθ1

{x˙=rθ˙1cos⁡θ1y˙=rθ˙1sin⁡θ1

\,\, \text{连杆II}

⎧⎩⎨⎪⎪x˙=lθ˙1cosθ1+r(θ˙1+θ˙2)cos(θ1+θ2)y˙=lθ˙1sinθ1+r(θ˙1+θ˙2)sin(θ1+θ2)

{x˙=lθ˙1cos⁡θ1+r(θ˙1+θ˙2)cos⁡(θ1+θ2)y˙=lθ˙1sin⁡θ1+r(θ˙1+θ˙2)sin⁡(θ1+θ2)

连杆I{

x

˙

=r

θ

˙

 

1


cosθ

1


y

˙


=r

θ

˙

 

1


sinθ

1



连杆II


 

x

˙

=l

θ

˙

 

1


cosθ

1


+r(

θ

˙

 

1


+

θ

˙

 

2


)cos(θ

1


2


)

y

˙


=l

θ

˙

 

1


sinθ

1


+r(

θ

˙

 

1


+

θ

˙

 

2


)sin(θ

1


2


)



所以质量微元速度为


{ 连杆I: v 1 2 = x ˙ 2 + y ˙ 2 = r 2 θ ˙ 1 2 连杆II: v 2 2 = x ˙ 2 + y ˙ 2 = l 2 θ ˙ 1 2 + r 2 ( θ ˙ 1 + θ ˙ 2 ) 2 + 2 l r θ ˙ 1 ( θ ˙ 1 + θ ˙ 2 ) cos ⁡ θ 2

⎧⎩⎨⎪⎪连杆I:v21=x˙2+y˙2=r2θ˙21连杆II:v22=x˙2+y˙2=l2θ˙21+r2(θ˙1+θ˙2)2+2lrθ˙1(θ˙1+θ˙2)cosθ2

{连杆I:v12=x˙2+y˙2=r2θ˙12连杆II:v22=x˙2+y˙2=l2θ˙12+r2(θ˙1+θ˙2)2+2lrθ˙1(θ˙1+θ˙2)cos⁡θ2


 

连杆I:v

1

2


=

x

˙

 

2

+

y

˙


 

2

=r

2

 

θ

˙

 

1

2


连杆II:v

2

2


=

x

˙

 

2

+

y

˙


 

2

=l

2

 

θ

˙

 

1

2


+r

2

(

θ

˙

 

1


+

θ

˙

 

2


)

2

+2lr

θ

˙

 

1


(

θ

˙

 

1


+

θ

˙

 

2


)cosθ

2




因此机器人动能为

E k = 1 2 ∫ m d m ⋅ v 2 = 1 2 ρ [ ∫ 0 l r 2 θ ˙ 1 2 d r + ∫ 0 l l 2 θ ˙ 1 2 + r 2 ( θ ˙ 1 + θ ˙ 2 ) 2 + 2 l r θ ˙ 1 ( θ ˙ 1 + θ ˙ 2 ) cos ⁡ θ 2 d r ] = 1 2 m l 2 [ ( 5 3 + cos ⁡ θ 2 ) θ ˙ 1 2 + ( 2 3 + cos ⁡ θ 2 ) θ ˙ 1 θ ˙ 2 + 1 3 θ ˙ 2 2 ] E_k=\frac{1}{2}\int_m{\mathrm{d}m\cdot v^2}\\=\frac{1}{2}\rho \left[ \int_0^l{r^2\dot{\theta}_{1}^{2}\mathrm{d}r}+\int_0^l{l^2\dot{\theta}_{1}^{2}+r^2\left( \dot{\theta}_1+\dot{\theta}_2 \right) ^2+2lr\dot{\theta}_1\left( \dot{\theta}_1+\dot{\theta}_2 \right) \cos \theta _2\mathrm{d}r} \right] \\=\frac{1}{2}ml^2\left[ \left( \frac{5}{3}+\cos \theta _2 \right) \dot{\theta}_{1}^{2}+\left( \frac{2}{3}+\cos \theta _2 \right) \dot{\theta}_1\dot{\theta}_2+\frac{1}{3}\dot{\theta}_{2}^{2} \right]

E

k


=

2

1


m


dm⋅v

2

=

2

1


ρ[∫

0

l


r

2

 

θ

˙

 

1

2


dr+∫

0

l


l

2

 

θ

˙

 

1

2


+r

2

(

θ

˙

 

1


+

θ

˙

 

2


)

2

+2lr

θ

˙

 

1


(

θ

˙

 

1


+

θ

˙

 

2


)cosθ

2


dr]

=

2

1


ml

2

[(

3

5


+cosθ

2


)

θ

˙

 

1

2


+(

3

2


+cosθ

2


)

θ

˙

 

1


 

θ

˙

 

2


+

3

1


 

θ

˙

 

2

2


]

3.2 势能计算与动力学方程

以基坐标系 x x x轴为零势能面,则系统总势能为

E p = − m g l [ 3 2 cos ⁡ θ 1 + 1 2 cos ⁡ ( θ 1 + θ 2 ) ] E_p=-mgl\left[ \frac{3}{2}\cos \theta _1+\frac{1}{2}\cos \left( \theta _1+\theta _2 \right) \right]

E

p


=−mgl[

2

3


cosθ

1


+

2

1


cos(θ

1


2


)]


对于任何机械系统.拉格朗日函数定义为系统总动能和势能之差,即

L = E k − E p = 1 2 m l 2 [ ( 5 3 + cos ⁡ θ 2 ) θ ˙ 1 2 + ( 2 3 + cos ⁡ θ 2 ) θ ˙ 1 θ ˙ 2 + 1 3 θ ˙ 2 2 ] + m g l [ 3 2 cos ⁡ θ 1 + 1 2 cos ⁡ ( θ 1 + θ 2 ) ] L=E_k-E_p\\=\frac{1}{2}ml^2\left[ \left( \frac{5}{3}+\cos \theta _2 \right) \dot{\theta}_{1}^{2}+\left( \frac{2}{3}+\cos \theta _2 \right) \dot{\theta}_1\dot{\theta}_2+\frac{1}{3}\dot{\theta}_{2}^{2} \right] +mgl\left[ \frac{3}{2}\cos \theta _1+\frac{1}{2}\cos \left( \theta _1+\theta _2 \right) \right]

L=E

k


−E

p


=

2

1


ml

2

[(

3

5


+cosθ

2


)

θ

˙

 

1

2


+(

3

2


+cosθ

2


)

θ

˙

 

1


 

θ

˙

 

2


+

3

1


 

θ

˙

 

2

2


]+mgl[

2

3


cosθ

1


+

2

1


cos(θ

1


2


)]


基于拉格朗日函数可得


{ ∂ L ∂ θ 1 = − m g l [ 3 2 sin ⁡ θ 1 + 1 2 sin ⁡ ( θ 1 + θ 2 ) ] ∂ L ∂ θ ˙ 1 = 1 2 m l 2 [ ( 10 3 + 2 cos ⁡ θ 2 ) θ ˙ 1 + ( 2 3 + cos ⁡ θ 2 ) θ ˙ 2 ] d d t ( ∂ L ∂ θ ˙ 1 ) = 1 2 m l 2 [ ( 10 3 + 2 cos ⁡ θ 2 ) θ ¨ 1 − 2 θ ˙ 1 θ ˙ 2 sin ⁡ θ 2 + ( 2 3 + cos ⁡ θ 2 ) θ ¨ 2 − θ ˙ 2 2 sin ⁡ θ 2 ]

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪∂L∂θ1=−mgl[32sinθ1+12sin(θ1+θ2)]∂L∂θ˙1=12ml2[(103+2cosθ2)θ˙1+(23+cosθ2)θ˙2]ddt(∂L∂θ˙1)=12ml2[(103+2cosθ2)θ¨1−2θ˙1θ˙2sinθ2+(23+cosθ2)θ¨2−θ˙22sinθ2]

{∂L∂θ1=−mgl[32sin⁡θ1+12sin⁡(θ1+θ2)]∂L∂θ˙1=12ml2[(103+2cos⁡θ2)θ˙1+(23+cos⁡θ2)θ˙2]ddt(∂L∂θ˙1)=12ml2[(103+2cos⁡θ2)θ¨1−2θ˙1θ˙2sin⁡θ2+(23+cos⁡θ2)θ¨2−θ˙22sin⁡θ2]


 

∂θ

1


∂L


=−mgl[

2

3


sinθ

1


+

2

1


sin(θ

1


2


)]

θ

˙

 

1


∂L


=

2

1


ml

2

[(

3

10


+2cosθ

2


)

θ

˙

 

1


+(

3

2


+cosθ

2


)

θ

˙

 

2


]

dt

d


(

θ

˙

 

1


∂L


)=

2

1


ml

2

[(

3

10


+2cosθ

2


)

θ

¨

 

1


−2

θ

˙

 

1


 

θ

˙

 

2


sinθ

2


+(

3

2


+cosθ

2


)

θ

¨

 

2


θ

˙

 

2

2


sinθ

2


]



{ ∂ L ∂ θ 2 = − 1 2 m g l sin ⁡ ( θ 1 + θ 2 ) − 1 2 m l 2 ( θ ˙ 1 2 + θ ˙ 1 θ ˙ 2 ) sin ⁡ θ 2 ∂ L ∂ θ ˙ 2 = 1 2 m l 2 [ ( 2 3 + cos ⁡ θ 2 ) θ ˙ 1 + 2 3 θ ˙ 2 ] d d t ( ∂ L ∂ θ ˙ 2 ) = 1 2 m l 2 [ ( 2 3 + cos ⁡ θ 2 ) θ ¨ 1 − θ ˙ 1 θ ˙ 2 sin ⁡ θ 2 + 2 3 θ ¨ 2 ]

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪∂L∂θ2=−12mglsin(θ1+θ2)−12ml2(θ˙21+θ˙1θ˙2)sinθ2∂L∂θ˙2=12ml2[(23+cosθ2)θ˙1+23θ˙2]ddt(∂L∂θ˙2)=12ml2[(23+cosθ2)θ¨1−θ˙1θ˙2sinθ2+23θ¨2]

{∂L∂θ2=−12mglsin⁡(θ1+θ2)−12ml2(θ˙12+θ˙1θ˙2)sin⁡θ2∂L∂θ˙2=12ml2[(23+cos⁡θ2)θ˙1+23θ˙2]ddt(∂L∂θ˙2)=12ml2[(23+cos⁡θ2)θ¨1−θ˙1θ˙2sin⁡θ2+23θ¨2]


 

∂θ

2


∂L


=−

2

1


mglsin(θ

1


2


)−

2

1


ml

2

(

θ

˙

 

1

2


+

θ

˙

 

1


 

θ

˙

 

2


)sinθ

2


θ

˙

 

2


∂L


=

2

1


ml

2

[(

3

2


+cosθ

2


)

θ

˙

 

1


+

3

2


 

θ

˙

 

2


]

dt

d


(

θ

˙

 

2


∂L


)=

2

1


ml

2

[(

3

2


+cosθ

2


)

θ

¨

 

1


θ

˙

 

1


 

θ

˙

 

2


sinθ

2


+

3

2


 

θ

¨

 

2


]



根据拉格朗日方程

{ τ 1 = d d t ( ∂ L ∂ θ ˙ 1 ) − ∂ L ∂ θ 1 τ 2 = d d t ( ∂ L ∂ θ ˙ 2 ) − ∂ L ∂ θ 2

⎧⎩⎨⎪⎪τ1=ddt(∂L∂θ˙1)−∂L∂θ1τ2=ddt(∂L∂θ˙2)−∂L∂θ2

{τ1=ddt(∂L∂θ˙1)−∂L∂θ1τ2=ddt(∂L∂θ˙2)−∂L∂θ2


 

τ

1


=

dt

d


(

θ

˙

 

1


∂L


)−

∂θ

1


∂L


τ

2


=

dt

d


(

θ

˙

 

2


∂L


)−

∂θ

2


∂L




求得机器人动力学模型为


τ = D 1 ( θ ) θ ¨ ⏟ 惯性力项 + D 2 ( θ ) θ ˙ 2 ⏟ 向心力项 + D 3 ( θ ) θ ˙ i j ⏟ 科氏力项 + D 4 ( θ ) ⏟ 重力项 ⇔ [ τ 1 τ 2 ] = D 1 ( θ ) [ θ ¨ 1 θ ¨ 2 ] + D 2 ( θ ) [ θ ˙ 1 2 θ ˙ 2 2 ] + D 3 ( θ ) [ θ ˙ 1 θ ˙ 2 θ ˙ 1 θ ˙ 2 ] + D 4 ( θ ) \boldsymbol{\tau }=\underset{\text{惯性力项}}{\underbrace{\boldsymbol{D}_1\left( \boldsymbol{\theta } \right) \boldsymbol{\ddot{\theta}}}}+\underset{\text{向心力项}}{\underbrace{\boldsymbol{D}_2\left( \boldsymbol{\theta } \right) \boldsymbol{\dot{\theta}}^2}}+\underset{\text{科氏力项}}{\underbrace{\boldsymbol{D}_3\left( \boldsymbol{\theta } \right) \boldsymbol{\dot{\theta}}_{ij}}}+\underset{\text{重力项}}{\underbrace{\boldsymbol{D}_4\left( \boldsymbol{\theta } \right) }}\\\Leftrightarrow \left[

τ1τ2

τ1τ2

\right] =\boldsymbol{D}_1\left( \boldsymbol{\theta } \right) \left[

θ¨1θ¨2

θ¨1θ¨2

\right] +\boldsymbol{D}_2\left( \boldsymbol{\theta } \right) \left[

θ˙21θ˙22

θ˙12θ˙22

\right] +\boldsymbol{D}_3\left( \boldsymbol{\theta } \right) \left[

θ˙1θ˙2θ˙1θ˙2

θ˙1θ˙2θ˙1θ˙2

\right] +\boldsymbol{D}_4\left( \boldsymbol{\theta } \right)

τ=

惯性力项

D

1


(θ)

θ

¨



+

向心力项

D

2


(θ)

θ

˙

 

2



+

科氏力项

D

3


(θ)

θ

˙

 

ij




+

重力项

D

4


(θ)



⇔[

τ

1


τ

2



]=D

1


(θ)[

θ

¨

 

1


θ

¨

 

2



]+D

2


(θ)[

θ

˙

 

1

2


θ

˙

 

2

2



]+D

3


(θ)[

θ

˙

 

1


 

θ

˙

 

2


θ

˙

 

1


 

θ

˙

 

2



]+D

4


(θ)


其中

{ D 1 ( θ ) = m l 2 [ 5 3 + cos ⁡ θ 2 1 3 + 1 2 cos ⁡ θ 2 1 3 + 1 2 cos ⁡ θ 2 1 3 ] D 2 ( θ ) = m l 2 [ 0 − 1 2 sin ⁡ θ 2 1 2 sin ⁡ θ 2 0 ] D 3 ( θ ) = m l 2 [ − sin ⁡ θ 2 0 0 0 ] D 4 ( θ ) = m g l [ 3 2 sin ⁡ θ 1 + 1 2 sin ⁡ ( θ 1 + θ 2 ) 1 2 sin ⁡ ( θ 1 + θ 2 ) ]

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪D1(θ)=ml2[53+cosθ213+12cosθ213+12cosθ213]D2(θ)=ml2[012sinθ2−12sinθ20]D3(θ)=ml2[−sinθ2000]D4(θ)=mgl[32sinθ1+12sin(θ1+θ2)12sin(θ1+θ2)]

{D1(θ)=ml2[53+cos⁡θ213+12cos⁡θ213+12cos⁡θ213]D2(θ)=ml2[0−12sin⁡θ212sin⁡θ20]D3(θ)=ml2[−sin⁡θ2000]D4(θ)=mgl[32sin⁡θ1+12sin⁡(θ1+θ2)12sin⁡(θ1+θ2)]


 

D

1


(θ)=ml

2

[

3

5


+cosθ

2


3

1


+

2

1


cosθ

2



 

3

1


+

2

1


cosθ

2


3

1



]

D

2


(θ)=ml

2

[

0

2

1


sinθ

2



 

2

1


sinθ

2


0


]

D

3


(θ)=ml

2

[

−sinθ

2


0


 

0

0


]

D

4


(θ)=mgl[

2

3


sinθ

1


+

2

1


sin(θ

1


2


)

2

1


sin(θ

1


2


)


]

 

3.3 动力学仿真

初始状态为 [ θ 1 θ 2 ] = [ 180 ° 0 ° ] \left[

θ1θ2

θ1θ2

\right] =\left[

180\degree0\degree

180\degree0\degree

\right] [

θ

1


θ

2



]=[

180°


]、 [ θ ˙ 1 θ ˙ 2 ] = [ 0 0 ] \left[

θ˙1θ˙2

θ˙1θ˙2

\right] =\left[

00

00

\right] [

θ

˙

 

1


θ

˙

 

2



]=[

0

0


],输入力矩为 [ τ 1 τ 2 ] = [ 2 0.5 ] \left[

τ1τ2

τ1τ2

\right] =\left[

20.5

20.5

\right] [

τ

1


τ

2



]=[

2

0.5


] 该初始条件下,平面2R机器人的角度、角速度、角加速度、能量、雅克比矩阵行列式数值波形图以及机器人末端执行器运动轨迹图如图所示


6c8c46ec15244968a7727533bb3640f1.png

🔥 更多精彩专栏

目录
打赏
0
0
0
0
25
分享
相关文章
基于QLearning强化学习的较大规模栅格地图机器人路径规划matlab仿真
本项目基于MATLAB 2022a,通过强化学习算法实现机器人在栅格地图中的路径规划。仿真结果显示了机器人从初始位置到目标位置的行驶动作序列(如“下下下下右右...”),并生成了详细的路径图。智能体通过Q-Learning算法与环境交互,根据奖励信号优化行为策略,最终学会最优路径。核心程序实现了效用值排序、状态转换及动作选择,并输出机器人行驶的动作序列和路径可视化图。
198 85
基于DWA优化算法的机器人路径规划matlab仿真
本项目基于DWA优化算法实现机器人路径规划的MATLAB仿真,适用于动态环境下的自主导航。使用MATLAB2022A版本运行,展示路径规划和预测结果。核心代码通过散点图和轨迹图可视化路径点及预测路径。DWA算法通过定义速度空间、采样候选动作并评估其优劣(目标方向性、障碍物距离、速度一致性),实时调整机器人运动参数,确保安全避障并接近目标。
149 68
使用PAI+LLaMA Factory 微调 Qwen2-VL 模型,搭建文旅领域知识问答机器人
本次教程介绍了如何使用 PAI ×LLaMA Factory 框架,基于全参方法微调 Qwen2-VL 模型,使其能够进行文旅领域知识问答,同时通过人工测试验证了微调的效果。
使用PAI+LLaMA Factory 微调 Qwen2-VL 模型,搭建文旅领域知识问答机器人
Seer:上海 AI Lab 与北大联合开源端到端操作模型,结合视觉预测与动作执行信息,使机器人任务提升成功率43%
Seer是由上海AI实验室与北大等机构联合推出的端到端操作模型,结合视觉预测与动作执行,显著提升机器人任务成功率。
73 20
Seer:上海 AI Lab 与北大联合开源端到端操作模型,结合视觉预测与动作执行信息,使机器人任务提升成功率43%
Cosmos:英伟达生成式世界基础模型平台,加速自动驾驶与机器人开发
Cosmos 是英伟达推出的生成式世界基础模型平台,旨在加速物理人工智能系统的发展,特别是在自动驾驶和机器人领域。
212 15
Cosmos:英伟达生成式世界基础模型平台,加速自动驾驶与机器人开发
基于的Qwen模型的智能客服Discord机器人,使用🐫 CAMEL、SambaNova、Firecrawl和Qdrant实现RAG Agent
基于Qwen模型的智能客服Discord机器人,使用CAMEL、SambaNova、Firecrawl和Qdrant实现RAG Agent。构建了一个能够处理复杂问题并能进行快速响应的强大聊天机器人。该机器人可在Discord平台上运行,支持实时对话和语义搜索,提供准确、全面的回答。项目包含详细的安装步骤、代码示例及集成指南,适合开发者快速上手。
RDT:清华开源全球最大的双臂机器人操作任务扩散基础模型、代码与训练集,基于模仿能力机器人能够自主完成复杂任务
RDT(Robotics Diffusion Transformer)是由清华大学AI研究院TSAIL团队推出的全球最大的双臂机器人操作任务扩散基础模型。RDT具备十亿参数量,能够在无需人类操控的情况下自主完成复杂任务,如调酒和遛狗。
180 22
RDT:清华开源全球最大的双臂机器人操作任务扩散基础模型、代码与训练集,基于模仿能力机器人能够自主完成复杂任务
Jim Fan全华人团队HOVER问世,1.5M小模型让机器人获潜意识!
在机器人技术领域,人形机器人的全身控制一直极具挑战。传统方法为不同任务(如导航、移动操作等)单独训练控制策略,限制了策略的可转移性。Jim Fan团队提出HOVER框架,通过全身运动模仿作为共同抽象,整合多种控制模式,实现无缝过渡,显著提升控制效率和灵活性。HOVER不仅为人形机器人应用带来巨大潜力,也为机器人技术发展提供新思路。论文地址:https://arxiv.org/abs/2410.21229
71 23
EMMA-X:新加坡科技设计大学推出具身多模态动作模型,使夹爪机器人具备空间推理和任务规划能力
EMMA-X是由新加坡科技设计大学推出的具身多模态动作模型,具备70亿参数,通过在链式思维推理数据上微调OpenVLA创建。该模型结合层次化的具身数据集,增强空间推理和任务规划能力。
122 3
EMMA-X:新加坡科技设计大学推出具身多模态动作模型,使夹爪机器人具备空间推理和任务规划能力
NeurIPS 2024:机器人操纵世界模型来了,成功率超过谷歌RT-1 26.6%
PIVOT-R是一种新型世界模型,专注于预测与任务相关的路点,以提高语言引导的机器人操作的性能和效率。该模型由路点感知世界模型(WAWM)和轻量级动作预测模块组成,辅以异步分层执行器(AHE),在SeaWave基准测试中表现优异,平均相对改进达19.45%,执行效率提高28倍。
85 26

热门文章

最新文章

AI助理

你好,我是AI助理

可以解答问题、推荐解决方案等