第
30
卷第
4
期
2008
年
7
月
文章编号：
1002-0446(2008)04-0326-07
机器人
ROBOT
Vol.30, No.4
July, 2008
基于动力学模型的½式移动机器人电机控制
陈晓鹏
1,2
，李成荣
2
，李功燕
2
，½杨宇
2
（1．北京理工大学，北京
100081； 2．中½科学院自动化研究所，北京
100190）
摘 要：针对移动机器人两路电机协同控制问题，提出基于动力学模型的½式移动机器人电机控制律
（DMMC）
．首先推导出质心½½不一定在几½中心的移动机器人运动学模型和动力学模型，并求解出两½速
度与力矩之间的非线性微分方程．然后，基于两½速度与力矩间非线性微分方程、电机电气方程和电机机电
方程，
推导出移动机器人系统状态方程．
最后采用极点配½得到
I
型状态反馈控制律．
仿真显示，
DMMC
法实
现了对输入指令的零稳态误差快速响应．
关键词：电机控制；移动机器人；建模；动力学；非完整约束；交叉耦合
中图分类号：TP249
文献标识码：A
Dynamic Model Based Motor Control for Wheeled Mobile Robots
CHEN Xiao-peng
1,2
, LI Cheng-rong
2
, LI Gong-yan
2
, LUO Yang-yu
2
(1.
Beijing Institute of Technology, Beijing
100081,
China;
2.
Institute of Automation, Chinese Academy of Sciences, Beijing
100190,
China)
Abstract:
A novel motor control law based on dynamic model (DMMC) is proposed for wheeled mobile robot to control
its two driving motors synchronously. First, kinematic model and dynamic model of the mobile robot, of which the mass
center position is arbitrary, are derived, and the nonlinear differential equation of speeds and torques of the two wheels is
derived. And then, system state equation of the mobile robot is derived based on the nonlinear differential equations of
speeds and torques, electrical equations and electrical-mechanical equations of the two driving motors. Finally, the pole
placement method is used to form a type I state feedback control law. Simulation shows that the DMMC controller can
respond to the input instructions quickly without static error.
Keywords:
motor control; mobile robot; modeling; dynamics; nonholonomic constraint; cross coupling
1
引言
(Introduction)
为实现移动机器人导航控制，需设计电机控
制律，½两路驱动电机响应输入速度指令或力矩
指令．
传统电机控制律采用两路
PID
校正环节分别
控制移动机器人两路电机，½它们对输入速度指
令或力矩指令信号进行响应．这种控制律缺乏两
路电机同步功½，若电机受到干扰，即½每路电
机成功跟随了速度信号，也不½很½地跟踪½迹
[1]
．为抑制干扰，文
[2]
采取了速度干扰补偿方法；
文
[3]
不以两路电机速度为控制参数，而是以移动
机器人线速度和角速度为控制参数；文
[4]
把两½
的速度差反馈给正向通道以提高抗干扰性；文
[1]
基金项目：½家
863
计划资助项目
(2007AA04Z227)．
收稿日期：2007-12-24
在每路电机前向校正环节基础上增加角速度补偿
校正环节．½然这些交叉耦合控制法在一定程度
上抑制了干扰，½是由于没有移动机器人的动力
学模型，限制了算法性½的进一步提高．
由于存在非完整约束，移动机器人的左右½
电机将受到约束反力产生的阻力矩½用，因此两
路电机具有很强的耦合性（详见第
2
节）
．不依据
被控对象模型设计的电机控制律，½然采用交叉
耦合反馈法，½对扰动仍较敏感．本文所述的移
动机器人两½电机协同控制方法（
DMMC
）
基于移
动机器人的动力学模型和电机模型，推导出移动
机器人系统状态方程，并在状态方程的基础上进
行极点配½反馈控制．仿真显示
DMMC
控制律成
功实现输入指令零稳态误差快速响应．
第
30
卷第
4
期
陈晓鹏等：基于动力学模型的½式移动机器人电机控制
327
2
移动机器人动力学分析
(Dynamics anal-
ysis for mobile robots)
为
J
Z
．机器人两½半径为
r
，左½电机输出½½动
惯量（包括电机½子和½½）是
J
ˆ
l
（左右½电机参
数中带尖½表示折算到输出½参数，不带尖½为
折算到电机½参数，下同）
，右½电机输出½惯量
（包括电机½子和½½）为
J
ˆ
r
，左右电机输出½力
ˆ ˆ
ˆ
ˆ
矩分别是
T
l
、
T
r
，左右½角½移分别为
ϕ
l
、
ϕ
r
．左
ˆ
ˆ
右½受到的
X
方向约束反力分别为
F
xl
、
F
xr
；两½
Y
方向受到的约束反力和为
F
y
．机器人广义½姿
˙ ˙
T
ˆ ˆ
ˆ ˆ
q
= [
x
c
y
c
θ ϕ
l
ϕ
r
]
T
，速度
v
=
ϕ
l
ϕ
r
．
2.1
移动机器人运动学方程
移动机器人是刚½，刚½各点以相同角速度
绕瞬心½动，易得质心在连½坐标系的速度表达
式
R
x
l
l
l
˙
r
c
ϕ
˙
l
ˆ
r
V
R
=
R
y
c
=
(1)
−d
d
˙
l
r
+
l
l
ϕ
r
˙
ˆ
˙
θ
−1
1
½化成世界坐标系下的速度表达式
−
sin
θ
cos
θ
0
0
0
V
R
1
ϕ
˙
ˆ
l
l
l
sin
θ
+
d
cos
θ
ϕ
r
˙
ˆ
1
(2)
l
l
cos
θ
−
d
sin
θ
cos
θ
x
˙
c
y
c
=
sin
θ
˙
˙
θ
0
图
1
Fig.1
移动机器人几½示意图
Geometrics of mobile robot
图
1
是差分驱动½式移动机器人俯视图
（图中
只绘出两驱动½） 世界静止坐标系为
xoy
，
，
连½坐
标系为
XOY
．连½坐标系
Y
½方向从右½中心指
向左½中心， ½与机器人两½共同½线平行，
Y
与
½½正前方垂直；
X
½方向从质心指向正前方，
X
½与世界坐标系
x
½夹角为
θ
；
原点
O
在移动机器
人质心
P
c
处，它与两½½线距离为
d
．质心在两
½½线上投½与左½距离为
l
l
，与右½距离为
l
r
，
在世界坐标系中坐标为
(x
c
,
y
c
)
．机器人整½（包括
½子及电机）质量为
m
，绕（经质心的）
z
½惯量
r(l
r
cos
θ
+
d
sin
θ
)
l
q
S
(q ) =
r(l
cos
l
r
+
l d
sin
θ
)
θ
−
l
l
r
+
l
l
l
r
cos
θ
+
d
sin
θ
r
=
l
sin
θ
−
d
cos
θ
l
r
+
l
l
r
−1
于是得到运动学方程
q v
˙
q
=
S
(q )v
(3)
其中
r
−
l
r
+
l
l
r
l
r
+
l
l
T
1 0
0 1
(4)
r(l
r
sin
θ
−
d
cos
θ
)
l
r
+
l
l
r(l
l
sin
θ
+
d
cos
θ
)
l
r
+
l
l
˙
由
(3)
式易验证广义速度
q
满足非完整约束方程
[5]
q
˙
A
(q )
q
=
0
(5)
分别在
x
、
y
、
z
方向及电机½方向对移动机器
人进行受力分析，机器人满足
x
、
y
方向力平衡、
z
方向力矩平衡、在电机½上的力矩平衡三大平衡
条件，于是得动力学方程
mx
c
−
(
F
xl
+
F
xr
)
cos
θ
+
F
y
sin
θ
=
0
ˆ
ˆ
¨
my
c
−
(
F
xl
+
F
xr
)
sin
θ
−
F
y
cos
θ
=
0
ˆ
ˆ
¨
¨ ˆ
ˆ
J
z
θ
+
F
xl
l
l
−
F
xr
l
r
+
F
y
d
=
0
¨
ˆ
ˆ
ˆ
J
ˆ
l
ϕ
l
+
F
xl
r
=
T
l
¨
ˆ
ˆ
ˆ
J
ˆ
r
ϕ
r
+
F
xr
r
=
T
r
其中
sin
θ
q
A
(q ) =
−
cos
θ
−
cos
θ
−
cos
θ
−
sin
θ
−
sin
θ
d
l
l
−l
r
0
0
r
0
0
r
(6)
(7)
2.2
移动机器人动力学方程
328
机 器
人
2008
年
7
月
整理成拉格朗日标准½式
q
¨
M q
=
E
τ
−
A
T
(q )
λ
(8)
q M
¨
τ
=
S
T
(q )M
q
(11)
其中
M
=
diag{m,
m, J
,
J
ˆ
,
J
ˆ
}
Z
l r
T
E
=
0 0 0 1 0
0 0 0 0 1
λ
=
F
y
F
xl
F
xr
T
ˆ
ˆ
τ
=
T
l
T
r T
ˆ ˆ
方程
(3)
、 ½式简单，
(11)
与传统的运动学方程及动
力学方程½式非常类似．
2.3
移动机器人速度力矩间非线性微分方程
对
(3)
式两端求导，并代入式
(11)
，得
S q M q
q M
˙
q v S q M q
˙
v
=
−(S
T
(q )M
S
(q ))
−1
S
T
(q )M
S
(q )v +(S
T
(q )M
S
(q ))
−1
τ
(12)
(9)
由式
(4)
、 得
(6)
q S q
A
(q )S (q ) =
0
(10)
q
方程
(8)
两端乘以
S
T
(q )
，并把式
(10)
代入，得简化
后的动力学方程
方程
(12)
包含了移动机器人动力学信息，经化简
得移动机器人两½速度与力矩非线性微分方程
ϕ
l
=
1
λ
l
T
l
+
2
λ
l
T
r
+
3
λ
l
ϕ
2
+
4
λ
l
ϕ
l
ϕ
r
+
5
λ
l
ϕ
2
ˆ ˙ ˙
ˆ ˆ
ˆ ˆ
ˆ ˙
ˆ ˙
¨
ˆ
l
ˆ ˆ
ˆ
r
ˆ
(13)
ϕ
=
1
λ
T
+
2
λ
T
+
3
λ ϕ
2
+
4
λ ϕ ϕ
+
5
λ ϕ
2
ˆ
r
˙
l
˙
r
ˆ
r
ˆ
l
ˆ
r
ˆ
r
ˆ
r
˙
ˆ
r
˙
¨
r
ˆ
ˆ
l
ˆ ˆ
ˆ
r
其中
(J
Z
+
md
2
)r
2
+
mr
2
l
2
+
J
ˆ
r
(l
l
+
l
r
)
2
1
λ
=
l
ˆ
l
2
)mr
4
+ (
J l
2
+
J l
2
)mr
2
+ (J +
md
2
)(
J
+
J
)r
2
+
J J
(l +
l
)
2
ˆ
l
ˆ
l
ˆ
r
ˆ
l
ˆ
r l r
ˆ
r r
(J
Z
+
md
Z
l
(J
Z
+
md
2
)r
2
−
mr
2
l
l
l
r
2
ˆ
λ
=
l
(J
Z
+
md
2
)mr
4
+ (
J
ˆ
r
l
r2
+
J
ˆ
l
l
2
)mr
2
+ (J
Z
+
md
2
)(
J
ˆ
l
+
J
ˆ
r
)r
2
+
J
ˆ
l
J
ˆ
r
(l
l
+
l
r
)
2
l
mdr
3
(J
Z
+
md
2
)r
2
−
mr
2
l
l
l
r
3
ˆ
λ
l
=
·
(l
l
+
l
r
)
2
(J
Z
+
md
2
)mr
4
+ (
J
ˆ
r
l
r2
+
J
ˆ
l
l
2
)mr
2
+ (J
Z
+
md
2
)(
J
ˆ
l
+
J
ˆ
r
)r
2
+
J
ˆ
l
J
ˆ
r
(l
l
+
l
r
)
2
l
4
2(J
Z
+
md
2
)r
2
+
mr
2
l
l
(l
l
−
l
r
) +
J
ˆ
r
(l
l
+
l
r
)
2
mdr
3
λ
=
−
ˆ
l
·
(l
l
+
l
r
)
2
(J
Z
+
md
2
)mr
4
+ (
J
ˆ
r
l
r2
+
J
ˆ
l
l
2
)mr
2
+ (J
Z
+
md
2
)(
J
ˆ
l
+
J
ˆ
r
)r
2
+
J
ˆ
l
J
ˆ
r
(l
l
+
l
r
)
2
l
5
ˆ
(J
Z
+
md
2
)r
2
+
mr
2
l
2
+
J
ˆ
r
(l
l
+
l
r
)
2
mdr
3
λ
=
l
l
·
2
(J +
md
2
)mr
4
+ (
J l
2
+
J l
2
)mr
2
+ (J +
md
2
)(
J
+
J
)r
2
+
J J
(l +
l
)
2
ˆ
l
ˆ
r l r
ˆ
r r
ˆ
l
ˆ
l
ˆ
r
(l
l
+
l
r
)
Z
Z
l
2
)r
2
−
mr
2
l l
(J
Z
+
md
1
ˆ
l r
λ
=
r
2
)mr
2
+ (J +
md
2
)(
J
+
J
)r
2
+
J J
(l +
l
)
2
ˆ
l
ˆ
r
ˆ
l
ˆ
r l r
(J
Z
+
md
2
)mr
4
+ (
J
ˆ
r
l
r2
+
J
ˆ
l
l
l
Z
2
)r
2
+
mr
2
l
2
+
J
(l +
l
)
2
ˆ
l l r
(J
Z
+
md
2
ˆ
r
λ
r
=
(J
Z
+
md
2
)mr
4
+ (
J
ˆ
r
l
r2
+
J
ˆ
l
l
2
)mr
2
+ (J
Z
+
md
2
)(
J
ˆ
l
+
J
ˆ
r
)r
2
+
J
ˆ
l
J
ˆ
r
(l
l
+
l
r
)
2
l
3
3
(J
Z
+
md
2
)r
2
+
mr
2
l
r2
+
J
ˆ
l
(l
l
+
l
r
)
2
λ
=
mdr
·
ˆ
r
(l
l
+
l
r
)
2
(J
Z
+
md
2
)mr
4
+ (
J
ˆ
r
l
r2
+
J
ˆ
l
l
2
)mr
2
+ (J
Z
+
md
2
)(
J
ˆ
l
+
J
ˆ
r
)r
2
+
J
ˆ
l
J
ˆ
r
(l
l
+
l
r
)
2
l
4
ˆ
2(J
Z
+
md
2
)r
2
+
mr
2
l
r
(l
r
−
l
l
) +
J
ˆ
l
(l
l
+
l
r
)
2
mdr
3
λ
=
−
r
·
(l
l
+
l
r
)
2
(J
Z
+
md
2
)mr
4
+ (
J
ˆ
r
l
r2
+
J
ˆ
l
l
2
)mr
2
+ (J
Z
+
md
2
)(
J
ˆ
l
+
J
ˆ
r
)r
2
+
J
ˆ
l
J
ˆ
r
(l
l
+
l
r
)
2
l
mdr
3
(J
Z
+
md
2
)r
2
−
mr
2
l
l
l
r
5
ˆ
λ
r
=
·
(l
l
+
l
r
)
2
(J
Z
+
md
2
)mr
4
+ (
J
ˆ
r
l
r2
+
J
ˆ
l
l
2
)mr
2
+ (J
Z
+
md
2
)(
J
ˆ
l
+
J
ˆ
r
)r
2
+
J
ˆ
l
J
ˆ
r
(l
l
+
l
r
)
2
l
(14)
速度状态方程是经过化简的移动机器人动力
学方程．
由公式易知移动机器人系统有如下性质：
1)
每个驱动½的加速度不仅与该½电机所½
加的力矩有关，还与另一电机所½加的力矩有关，
而且与两½速度的二次方有关，并且它们之间是
线性组合，组合系数（见公式
(14)
）是仅与机器人
模型参数（包括
J
Z
、
J
ˆ
l
、
J
ˆ
r
、
m
、
r
、
d
、
l
l
、
l
r
）有关的
常数，
而与速度、
加速度无关，
如果这些与模型相
关的参数没有发生改变，
则状态方程不会变化，
系
统属于非线性系统．
2)
如果质心不½在两½½心，即
d
=
0
，状态
方程包含速度的二次方项，系统本身是强耦合非
线性系统，左电机的输出力矩会½响右½的加速
度，右½的输出力矩会½响左½加速度，左½速
度会对右½速度造成½响，右½速度也会对左½
速度造成½响．
3)
若质心½在两½½心上，即
d
=
0
，方程化
简成
ϕ
=
1
λ
T
+
2
λ
T
ˆ
l
ˆ
l
ˆ
l
ˆ
r
¨
l
ˆ
(15)
ϕ
=
1
λ
T
+
2
λ
T
ˆ
r
ˆ
l
ˆ
r
ˆ
r
¨
r
ˆ
这时系统变成线性定常系统，通过一定的线
性变换可以实现系统的解耦控制．
第
30
卷第
4
期
陈晓鹏等：基于动力学模型的½式移动机器人电机控制
329
3 DMMC
法
(DMMC algorithm)
DMMC
法利用移动机器人动力学的信息进行
设左、
右½电机的½矩电流常数
（与电压速度
常数相等）分别为
C
ml
、
mr
，电机回路电感分别为
C
L
l
、
L
r
，回路电阻分别为
R
l
、
R
r
，电气时间常数分
电机控制．此方法基于动力学方程
(13)
，利用直
流电机的电气和机电微分方程推导系统状态方程，
然后在系统状态方程基础上进行
I
型极点配½反
馈控制．
3.1
移动机器人系统状态方程
首先将所有输出½参数½换为电机½参数．
电机½角速度用
ϖ
l
、
ϖ
r
表示．设两电机减速器的
减速比½为
i
，于是式
(13)
变换为
ϖ
=
1
λ
T
+
2
λ
T
r
+
3
λ ϖ
2
+
4
λ ϖ ϖ
r
+
5
λ ϖ
2
˙
l
l l
l
l l
l l
l r
ϖ
=
1
λ
T
+
2
λ
T
+
3
λ ϖ
2
+
4
λ ϖ ϖ
+
5
λ ϖ
2
˙
r
r l
r r
r
l
r
l
r
r
r
别为
τ
l
、
τ
r
，回路电流分别为
I
l
、
I
r
，输入电压分别
为
U
l
、
U
r
，电机实际输出力矩分别为
T
l
、
T
r
，电机
产生力矩分别为
T
ml
、
mr
，反电动势分别为
E
l
、
r
，
T
E
左、右½电机恒值阻力矩分别为
T
cl
、
T
cr
，一次方
阻力矩分别为
T
ϖ
l
、
ϖ
r
，一次方阻力矩系数分别为
T
f
ϖ
l
、
f
ϖ
r
，恒值阻力矩电流分别为
I
cl
、
cr
，它们之间
I
有如下关系式：
τ
l
=
L
l
R
l
T
ml
=
C
ml
I
l
E
=
C
ϖ
l
ml l
˙
U
l
−
E
l
=
R
l
(I
l
+
τ
l
I
l
)
T
cl
=
C
cl
I
cl
T
=
f
ϖ
ϖ
l
ϖ
l l
T
l
=
T
ml
−
T
cl
−
T
ϖ
l
τ
r
=
L
r
R
r
T
mr
=
C
mr
I
r
E
r
=
C
mr
ϖ
r
˙
U
r
−
E
r
=
R
r
(I
r
+
τ
r
I
r
)
(18)
T
cr
=
C
cr
I
cr
T
ϖ
r
=
f
ϖ
r
ϖ
r
T
r
=
T
mr
−
T
cr
−
T
ϖ
r
(16)
其中的系数½换关系为
ˆ
1
λ
l
=
i
2
×
1
λ
l
2
λ
l
=
i
2
×
2
λ
l
ˆ
3
λ
=
1/i
×
3
λ
ˆ
l
l
4
λ
=
1/i
×
4
λ
ˆ
l
l
5
λ
=
1/i
×
5
λ
ˆ
l
l
1
λ
r
2
λ
r
3
λ
r
4
λ
r
5
λ
r
ˆ
=
i
2
×
1
λ
ˆ
=
i
2
×
2
λ
r
r
ˆ
=
1/i
×
3
λ
r
ˆ
=
1/i
×
4
λ
r
ˆ
=
1/i
×
5
λ
r
(17)
整理式
(16)
、
(18)
得
I
(t) =
−
1
I
(t)
−
1
E
(t) +
1
U
(t)
˙
l
l
l
l
τ
l
R
l
τ
l
R
l
τ
l
˙
I
r
(t) =
−
1
I
r
(t)
−
1
E
r
(t) +
1
U
r
(t)
τ
r
R
r
τ
r
R
r
τ
r
3
4
5
˙
2
E
l
(t) =
1
λ
l
C
ml
I
l
(t) +
2
λ
l
C
ml
C
mr
I
r
(t) +
λ
l
E
2
(t) +
λ
l
E
l
(t)E
r
(t) +
λ
l
C
ml
E
r2
(t)
2
C
ml l
C
mr
C
mr
C
ml
2
−
1
λ
l
C
ml
I
cl
−
2
λ
l
C
ml
C
mr
I
cr
−
1
λ
l
f
ϖ
l
E
l
(t)
−
2
λ
l
f
ϖ
r
E
r
(t)
C
mr
3
4
5
˙
E
(t) =
1
λ
C C I
(t) +
2
λ
C
2
I
(t) +
λ
r
C
mr
E
2
(t) +
λ
r
E
(t)E (t) +
λ
r
E
2
(t)
r
r ml mr l
r mr r
r
l
l
2
C
ml
C
mr r
C
ml
C
mr
f
ϖ
l
2
−
1
λ
r
C
ml
C
mr
I
cl
−
2
λ
r
C
mr
I
cr
−
1
λ
r
E
l
(t)
−
2
λ
r
f
ϖ
r
E
r
(t)
C
ml
式
(19)
可写成移动机器人系统状态方程
1
1
1
−
−
0
0
0
˙
I
l
(t)
I
l
(t)
τ
l
R
l
τ
l
R
l
τ
l
˙
1
λ
C
2
2
λ
C C
2
C
ml
f
ϖ
r
−
1
λ
l
f
ϖ
l
l ml
l ml mr
−
λ
l
E
l
(t)
0
C
mr
E
l
(t)
+
0
=
˙
1
1
1
I
r
(t)
I
r
(t)
0
0
−
0
−
τ
r
R
r
τ
r
R
r
τ
r
C
mr
f
ϖ
l
˙
r
(t)
E
r
(t)
E
1
1
λ
C C
2
λ
C
2
0
0
−
2
λ
r
f
ϖ
r
r ml mr
−
λ
r
r mr
C
ml
0
3
4
λ
5
λ
C
λ
l 2
l
l ml 2
2
E
l
(t) +
E
l
(t)E
r
(t) +
E
r
(t)
−
1
λ
l
C
ml
I
cl
−
2
λ
l
C
ml
C
mr
I
cr
C
2
C
mr
C
mr
+
ml
0
3
4
λ
5
λ
λ
r
C
mr
r
r 2
2
E
2
(t) +
E
l
(t)E
r
(t) +
E
r
(t)
−
1
λ
r
C
ml
C
mr
I
cl
−
2
λ
r
C
mr
I
cr
l
2
C
ml
C
mr
C
ml
(19)
U
l
U
r
(20)
330
机 器
人
2008
年
7
月
˙ ˙
ˆ ˆ
若选取速度
1
y
=
v
=
ϕ
l
ϕ
r
T
为输出量，
则输
给定期望输出
y
r
，根据状态方程
(23)
设计
I
型
反馈控制律（设计细节参考文
[6]
）
，令
˙
ξ
=
y
r
−
y
=
y
r
−C
x
C
出方程为
1
y
=
˙
ˆ
ϕ
l
˙
ˆ
ϕ
r
0
0
=
1
iC
ml
0
0
0
0
1
iC
mr
I
l
(t)
E
l
(t)
I
r
(t)
E
r
(t)
(21)
(25)
则状态方程可以改写成增广系统状态方程
˙
x
(t)
x
(t)
¯
¯
¯
=
A
+
B u
(t) +
B
r
y
r
(t)
˙
(t)
ξ
ξ
(t)
其中
A
C
−C
0
0
B
0
0
I
¯
A
=
¯
B
=
¯
B
r
=
(26)
ˆ ˆ
也可以选取力矩
2
y
=
τ
=
T
l
T
r
T
为输出量，于是
I
l
(t)
输出方程为
2
(27)
y
=
ˆ
T
l
ˆ
T
r
iC
ml
0
0
0
0
iC
mr
=
0
E
l
(t)
(22)
0
I
r
(t)
E
r
(t)
方程
(20)
是基于动力学模型和电机模型的移动机
器人电机状态方程，
其输入量是电机两端电压，
状
态量为两½的电流和反电动势．
3.2 DMMC
电机控制算法
根据式
(26)
设计状态反馈控制律
x
x
=
−
K
x
K
ξ
K
u
=
−K
ξ
ξ
(28)
¯ ¯
其中反馈矩阵
K
对应的系统矩阵为
A B
，
K
可
通过极点配½法或
LQG
法求解．把式
(28)
代入式
(26)
可得闭环系统状态方程变为
x
(t)
K
A
−
B K
x
−K
ξ
x
(t)
0
˙
=
+
y
r
(t)
˙
ξ
(t)
C
−C
0
ξ
(t)
I
y
= [C
0
]
x
(t)
C
ξ
(t)
(29)
舍弃式
(20)
中的非线性部分，状态方程具有
如下½式
x
=
A x
+
B u
˙
y
=
C x
(23)
其中
A
=
1
−
τ
l
1
λ
2
l
C
ml
1
−
R
l
τ
l
−
1
λ
l
f
ϖ
l
0
C
mr
f
ϖ
l
−
1
λ
r
C
ml
2
λ
0
l
C
ml
C
mr
0
1
λ
C C
r ml mr
1
−
τ
r
2
λ
C
2
r mr
C
ml
f
ϖ
r
−
2
λ
l
C
mr
1
−
R
r
τ
r
2
λ
f
−
r
ϖ
r
0
可以通过计算式
(29)
的响应曲线得到系统的动静
态特性．
综上所述可以得到
DMMC
法的步骤
1)
测出机器人的模型参数
J
Z
、
ˆ
l
、
ˆ
r
、 、 、 、
J J m r d
l
l
、
r
；
l
2)
查电机手册得到左右½电机参数
C
ml
、
mr
、
C
i
、
l
、
r
、
l
、
r
，估计阻尼系数
f
ϖ
l
、
f
ϖ
r
和恒阻力
R R L L
B
=
1
R
l
τ
l
0
0
0
0
1
R
r
τ
r
0
0
0
矩电流
I
cl
、
cr
；
I
ˆ
3)
根据式
(14)
计算
λ
系数，并根据式
(17)
将
其½换为电机½系数；
4)
根据式
(24)
计算矩阵
A
、 、 ；
B C
¯ ¯
5)
根据式
(27)
计算增广系统矩阵
A B
，并
1
C
=
0
0
1
0
iC
ml
0
0
2
C
=
iC
ml
0
1
iC
mr
0 0 0
0
iC
mr
0
采用极点配½法或
LQG
法计算反馈矩阵
K
；
6)
在每个采样周期内，给定输入
y
r
，执行
7)
∼9)
操½，完成闭环控制；
7)
通过电流采样、速度采样获取状态变量
x
，
(24)
根据式
(25)
估计状态变量
ξ
；
8)
根据式
(28)
计算出所需的输入量，即两路
状态量、 输入量 、 输出量分别为
x
= [
I
l
(t)
E
l
(t)
I
r
(t)
E
r
(t) ]
T
、
u
= [U
l
U
r
]
T
、
y
=
1
y
或
y
=
2
y
．
电机需要的电压值（或
PWM
值）
；
京公网安备 11010802033920号
Copyright © 2005-2024 EEWORLD.com.cn, Inc. All rights reserved
评论