旋转式水下机器人的空间运动方程

3. 1 基准坐标系 AXeYeZe 与惯性坐标系 AX 0Y0 Z0 之间的关系
由图 1 和图 2 可知基准坐标系与惯性坐标系之间的关系为
xe
1 0 0 x0
x0
ye = 0 0 1 y 0 = H e y 0
( 1)
ze
0 - 1 0 z0
z0
其中 x e, y e, z e, x 0, y 0, z 0 分别为基准坐标系和惯性坐标系上相应各坐标轴的单位矢量, 其它坐
了所采用的各种坐标系及其变换关系; 其次, 把机器人做为刚体, 分析机器人在运动中的受力
情况, 得到载体坐标系上的机器人运动方程; 从而推出旋转式水下机器人在惯性坐标系下的运
动方程. 最后, 通过控制舵片的偏转来控制机器人的运动, 进行了计算机仿真, 充分证明了所推
导的机器人运动方程.
2 采用的坐标系( 示于图 2)
度 !v 旋转, 得到 OX 1 Y 1Z1 ( 如图 4 所示) . 两个坐标系之间的坐标变换关系用矩阵表示为
x1
1
0
0
x1
y 1 = 0 cos!v t sin!vt y 2
( 7)
z1
0 - sin!vt cos!vt z2
3. 4 载体坐标系 OX1 Y1Z1 与速度坐标系 OXYZ 的关系 在随机器人自旋的坐标系上, 设冲角为 ∀和 #. 先使载体坐标系与速度坐标系重合, 然后
!y =
s in ∀
co s∀
0 !y 1 - cos∀#
( 21)
!z
- cos∀sin# sin∀sin# cos# !z 1 - ∀
当 ∀, # 1 时有
第 19 卷第 1 期
刘 丽等: 旋转式水下机器人的空间运动方程
17
!x
1 - ∀ # !x1 - ∀#
!y = ∀ 1 0 !y1 - #
F = Fx x + F yy + F zz
( 16)
由动量定理知
mv = F
( 17)
而
mv = mvx + mvx
( 18)
且有
x = !zy - !yz
( 19)
所以
mv = mvx + mv!z y - mv!y z
( 20)
由速度坐标系与载体坐标系的关系有
!x
cos∀cos# - sin∀cos# sin# !x 1 - sin∀#
的夹角, 也就是 O X 1 轴和 OX ′之间的夹角. 横倾角 ——机器人对称面 X 1OZ1 与通过 OX 1 的
平面 X 1O Z0 之间的夹角, 即 X 1 OZ1 平面的法线 O Y 1 与 X 1 OZ0 之间的夹角( 见图 3) . 各角度的
正向都以惯性坐标轴为起点按右手螺旋法则确定. 因此, 惯性坐标系通过 3 次旋转变换可与载
在载体坐标系上, 设机器人绕 OX 1, O Y 1 和 OZ1 轴的角速度分别为 !x1, !y1 , !z 1, 机器人相 对于各轴的动量矩分别为 j x1 !x1 , j y1 !y 1, j z 1 !z 1 ( j 中含附加转动惯量) ; 绕各轴的外力矩分别
为 M x1, M y1 , M z 1. 则总的动量矩为
关键词 旋转式水下机器人, 坐标系, 坐标变换, 运动方程
1 引言
本文讨论的旋转式水下机器人( 如图 1 所示) , 它的 外形为流线形细长体, 尾部有一个螺旋桨推动其向前运
舵
鳍
动; 机器人前部有一对斜置的鳍, 且两个鳍的方向相反, 在机器人运动的过程中, 由于鳍倾角的存在, 通过水动
· 控制轴
舵
鳍
O( A)
x1 x x0
x1
10
0
x1
y′= 0 co s - sin y1 ( 4)
z′ 0 sin cos
z1
综合( 2) ～( 4) 式, 得到惯性坐标系与载
体坐标系之间的坐标变换关系[ 1] :
z0
z
z1
图3 空间姿态角
x0
x1
y 0 = H b y1
( 5)
z0
z1
cos co s cos sin sin - sin cos co s sin cos + sin sin
标系的定义相同, 不再重复说明.
3. 2 惯性坐标系 AX 0Y0Z0 与载体坐标系 OX1 Y1Z1 间的关系
载体坐标系相对于惯性坐标系的 3 个姿态角 , , 定义如下: 纵倾角 —— 载体 OX 1 轴
在平面 X 0A Y 0 上的投影 OX ′与 A X 0 轴之间的夹角. 艏向角 ——OX 1 轴与平面 X 0A Y 0 之间
速度坐标系取 O 为原点; OX 轴取机器人的速度方向; OY 轴在载体坐标系的 X 1OY 1 平面 内, 垂直于速度矢量, 即垂直于 OX 轴, 其指向在 OY 1 一侧; OZ 轴垂直于 X OY 平面, 与 OX 轴、OZ 轴共同组成右手坐标系. 速度坐标系也随机器人的自旋而转动.
3 坐标变换关系
将速度坐标系绕 OY 轴旋转 # 角, 得到 OX ′1Y Z1, 再绕 O Z1 轴旋转 ∀角, 得到 OX 1Y 1Z1 ( 如图 5 所示) . 两个坐标系之间的坐标变换关系用矩阵表示为
x1
co s∀co s# sin∀ - co s∀sin# x
x
y 1 = - sin∀co s# cos∀ sin∀sin# y = H h y
( 22)
从而有
!z
- # ∀# 1 !z 1 - ∀
!y = ∀!x1 + !y1 - # - ∀2 # ≈ ∀!x1 + !y1 - # !z = - #!x1 + ∀## + ∀#!y1 - ∀## + !z 1 - ∀≈- #!x 1 + !z 1 - ∀
( 23) ( 24)
将各参量代入( 17) 式, 并整理得
部; OY 1 轴与机器人纵轴和控制机构转轴构成的平面垂直, 在机器人开始自转的瞬间, 控制机 构转轴处于水平位置, O Y 1 轴指向上; OZ1 轴与控制机构转轴平行, 其指向按右手坐标系确定. 载体坐标系与机器人固联, 随机器人的自旋而旋转. 2. 4 半载体坐标系 OX 1 Y2Z2
半载体坐标系的原点取为 O 点; OX 1 轴为机器人的纵轴, 指向机器人的头部; OY 2 轴在包 含机器人纵轴的铅垂面内, 与机器人的纵轴垂直, 指向上; OZ2 与 OX 1, OY 2 轴共同组成右手坐 标系. 半载体坐标系不随机器人转动. 2. 5 速度坐标系 OXYZ
j ! x1 x 1 - j y 1w y1 !z1 + j z 1!y 1!z 1 = M x 1
( 13)
j ! y1 y 1 + j x1 w x1 !z1 - j z 1!x 1!z 1 = M y1
( 14)
j ! z1 z 1 - j x 1w ! x1 y1 + j y1!x 1!y1 = M z 1 方程组中的第一个方程表示水下机器人自旋角速度的变化, 机器人自旋角速度可通过调节鳍 倾角来控制, 为简化讨论, 这里不予考虑, 将第二、三个方程中出现的 !x1 做固化处理. 取 !x1 = !v, 并用矩阵表示载体坐标系上的力矩方程为
14 y0
机 器 人
y y1 y2
1997 年 1 月
A z0
( 1)
x0 ye
A
ze ( 2)
xe
O
x
x1 z2
z1 z ( 3)
图 2 采用的坐标系 ( 1) 惯性坐标系, ( 2) 基准坐标系, ( 3) 载体坐标系、半载体坐标系和速度坐标系
2. 3 载体坐标系 OX1 Y1Z1 取水下机器人的质心 O 为载体坐标系的原点; OX 1 轴为机器人的纵轴, 指向机器人的头
其中
= j x1 !x1x1 + j x 1!x 1x1 + j y1 !y1y 1 + j y 1!y1 y1 + j z 1!z 1z 1 + j z 1 !z1 z1
( 12)
x1 = !z 1 y1 - !y 1z 1, y 1 = !x1z 1 - !z 1x1 , z1 = !y 1x1 - !x1y 1 将各参量代入( 11) 式, 并整理得
( 8)
z1
sin #
0
cos#
z
z
y1 y2 !vt
O !v t z2 z1
y1 y
x1( 纵轴)
∀#
∀
x1
x1
O #∀
#
x
( 速度矢量)
z z1
图 4 载体坐标系与半载体坐标系 图 5 载体坐标系与速度坐标系
16
机 器 人
1997 年 1 月
4 载体坐标系上的机器人运 动方程
∀
0 -1
# = !v 1 0
∀ #+
01 10
!y1 !z 1
+
1 mv
-1 0
0 1
Fy Fz
下面, 我们再来考虑载体的受力状态[ 2] . 如图 6
y1
所示, 机器人受到的绕 OZ1 轴的外力矩包括:
( 1) 由于载体以 !z1 摆动而引起的阻力力矩, 其
∀
大小为
∀
1 2
C
nwz
1
!z
1
力的作用对机器人产生一个扭矩, 使其绕纵轴旋转; 机
器人后部设置有一对舵, 改变脉冲调宽信号控制舵的偏
转, 将对机器人产生不同大小和方向的力, 在机器人的 运动过程中, 主要是通过对舵片偏转的控制来控制机器
图1 机器人的外形结构
人的运动.
由于此种机器人是旋转 式前进的, 载体坐标系随着机器人的自旋而旋转, 本文首先讨论
H = j x 1 !x1x1 + j y 1 !y1y 1 + j z 1 !z 1 z1
( 9)
总的外力矩为
由动量定理知
M = M x1 x1 + M y1y 1 + M z1 z1
( 10)
H= M
( 11)
而
H=
d( j x1 !x1x1 +
j y 1!y1 y1 + dt
j z 1!z 1z 1)
体坐标系重合, 三次坐标变换可表示为
第 19 卷第 1 期
刘 丽等: 旋转式水下机器人的空间运动方程
15
x0 y0 = z0 x′ y′= z0
cos - sin 0
sin cos 0
0
01
cos 0 sin
0 10
- sin 0 cos
x′ y′ ( 2) z0 x1 y′ ( 3) z′
y y′ y1
!y1 !y1 =
M y1 jy1
M z1 j z1
0
j y1 - j x1 j z 1!v
jx1 -பைடு நூலகம்jz1
j y1 !v 0
!y1
!z 1
( 15)
设 m 为机器人的质量( 含附加质量) . v 为机器人的前进速度. 在速度坐标系上, 设机器人所受
外力 F 在 O X , O Y , OZ 轴上的分量分别为 Fx , Fy , F z , 即有
为使从事水下机器人研究的人员阅读方便, 采用国际上统一使用的基准坐标系 A X eY eZe. 取 A 点为原点; A Ze 轴指向地心; X eA Y e 平面垂直于 A Ze 轴, A X e 轴正方向取水平向右, A Y e 轴与 A X e 轴, A Ze 轴共同构成右手坐标系.
自然科学基金资助项目. 1995- 08- 15 收稿
H b = sin cos sin sin sin + co s cos sin sin cos - cos sin
( 6)
- sin
cos sin
cos cos
3. 3 载体坐标系 OX1 Y1Z1 与半载体坐标系 OX 1Y2 Z2 的关系
先使载体坐标系与半载体坐标系重合, 然后将半载体坐标系绕 OX 1 轴以机器人自旋角速
2. 1 惯性坐标系 AX 0Y0Z0 取海中某一点, 一般取机器人初始工作点 A 为原点建立惯性坐标系; A Y 0 轴指向上( 背向 地心) ; A X 0 轴水平地指向任一方向, 这里取水平向右为 A X 0 轴正方向; A Z0 轴与 X 0A Y 0 平面 垂直, 指向按右手坐标系确定. 2. 2 基准坐标系 AXeYeZe
mv = Fx
mv ( - #!x1 + !z 1 - ∀) = Fy
( 25)
- mv ( ∀!x1 + !y 1 - #) = Fz 其中的第一式表示机器人前进速度的变化, 机器人的前进速度可通过调节螺旋桨来控制, 为了
简化讨论, 这里不予考虑. 对第二式、第三式中出现的 v 做固化处理, 且取 !x 1= !v, 整理上式并 用矩阵表示为
第 19 卷第 1 期 1997 年 1 月
机器人 ROBOT
V ol. 19, N o. 1 Jan. , 1997
旋转式水下机器人的空间运动方程
刘 丽
华克强
( 哈尔滨工程大学水声研究所 150001) ( 中国民用航空学院 300300)
摘 要 本文提出了一种旋转自主式水下机器人 的构想, 简要 说明了其系统结构 和工作原理, 分 析了其在空间运动中所受力和力矩的情况, 推导出其空间运动方程. 通过计算 机仿真对所推导的水 下 机器人空间运动方程进行了验证.