工业机器人运动车体位姿计算方法研究

收稿日期:2000210230.作者简介:石广田(19622),男,教授,博士研究生.
文章编号:045522059(2001)022*******
工业机器人运动车体位姿计算方法研究
石广田1,俞焕然1,李重庵1,朱常琳2
(1.兰州大学物理科学与技术学院力学系,甘肃兰州　730000;
2.兰州铁道学院机械系,甘肃兰州　730070)
摘要:介绍了工业机器人视觉导航中的运动车体位姿的计算方法,论述了车体坐标系和摄像机坐标系之间的变化关系,推导出了车体坐标系和摄像机坐标系之间的变换矩阵,并进行了实验.通过实验确定了实验方案中的目标物体的位置和摄像机的运动参数,为实验中所使用的轮式移动机器人的运动导航打下了基础.确定了工业机器人小车相对于目标的位姿,就可以为移动车体提供导航与定位的依据.
关键词:机器人;导航与定位;位姿;变换矩阵
中图分类号:T P 242.2 文献标识码:A
工业机器人的视觉导航与定位在机械制造自动化中有着重要意义.确定了机器人小车相对目标的位姿,移动车体的导航与定位就有了依据[1,2].
附图　坐标系统F ig .　Coo rdinate system 1　坐标系统的建立
如附图所示,以目标体几何中心为原点建立坐标系
(X w ,Y w ,Z w ),以摄像机的投影为基础建立摄像机坐标系
(X c ,Y c ,Z c ),以车体后轮轴中心为原点建立车体坐标系
(X t ,Y t ,Z t ).它们之间的关系可以用旋转矩阵R 与平移向
量t 来描述.空间一点P 在3个坐标系下的齐次坐标分别
为(x w ,y w ,z w ,1)T ,(x t ,y t ,z t ,1)T 和(x c ,y c ,z c ,1)T ,下标w
表示世界坐标系,下标t 表示车体坐标系,下标c 表示摄像
机坐标系.各坐标系之间的关系分别为x t
y t
z t 1=R t-w t t-w 0T
1x w y w z w 1=M t-w x w y w z w 1,(1)x c
y c
z c 1
=R c-t t c-t 0T 1x t y t z t 1=R const t const 0T 1x t y t z t 1=M c-t x t y t z t 1
.(2)
第37卷第2期
2001年4月兰州大学学报(自然科学版)Jou rnal of L anzhou U n iversity (N atu ral Sciences )V o l .37N o.2A p r .2001
因为车体与摄像机之间的位置相互固定,所以,R c-t =R const ,t c-t =t const ,M
c-t =M const .x
c
y c
z c 1=M c-t x t y t z t 1
=M c-t M t-w x w y w z w 1=M const M t-w x w y w z w 1=M c-w x w y w z w 1,(3)
即,
摄像机坐标系与世界坐标系存在如下变换关系x c
y c
z c
1=R t 0T 1x w y w z w 1=
M x w y w z w 1
.(4)
式中:R t-w ,R c-t ,R 为3×3旋转矩阵;t t-w ,t c-t ,t 为三维平移向量;0=(0,0,0)T ,M t-w ,M c-t ,M 为4×4的矩阵.由M =M c-w =M const M t-w 可知,M ,M const 求出后,M t-w 可以被确定.而M t-w 正是所要求的车体位姿信息.
2　摄像机坐标系与车体坐标系之间的变换矩阵的计算
要求出R t-w ,关键是要求出M const ,即摄像机坐标系与车体坐标系之间的变换矩阵M c-t .目标物体相对于摄像机坐标系的位置可用摄像机标定方法求得.
用A ,B ,C ,D ,X 表示4×4矩阵,分别描述某两个坐标系之间的相对方位.如果这两个坐标系之间的相对方位由旋转矩阵R a 与平移向量t a 描述(下标a 表示矩阵的名称),则
A =R a t a
0T 1.
通过摄像机标定方法可以求得摄像机的外参数(摄像机与目标物体的相对方位).将实验小车运动前、后两个位置的摄像机外参数用A 与B 表示.用C 表示实验小车摄像机坐标系运动前后的相对方位,则C =AB -1.用D 表示实验小车车体坐标系运动前后的相对方位,它可以测量出来,因此是已知参数.用X 表示摄像机和运动车体坐标系之间的相对方位.
设空间某点P 在摄像机和运动车体坐标系中运动前后的坐标分别为P c1,P c2,P t1,P t2,则有
如下关系P c1=CP c2,
(5)P c1=X P t1,
(6)P t1=D P t2,
(7)P c2=X P t2.(8)
由式(5)与(8)得P c1=CX P t2,
(9)由式(6)与(7)得P c1=XD P t2,
(10)比较式(9)与(10)得到CX =XD .
(11)式(11)就是系统的基本方程式,其物理依据就是实验小车移动前后与摄像机的相对位置X 不变.式(11)中,X 是待求参数,C 由两次摄像机定标的外参数得到,因此,C 和D 均是已知参数.
如果将式(11)中的各4×4矩阵分别用相应的旋转矩阵与平移向量写出,则式(11)可写成R c t c 0T
1R t
0T
1=R t 0T 1R d t d
0T 1.(12)35第2期 石广田等:工业机器人运动车体位姿计算方法研究
展开上式得R c R=RR d,(13)
R c t+t c=R t d+t,(14)需求解的是R与t.
由于R c=RR d R-1,所以R c与R d是相似矩阵,有相同的特征值.由于旋转矩阵的特征值由旋转角惟一确定,所以R c与R d所定义的旋转角必然相等.若将旋转角记作Η,则将式(13)写成
R c(k c,Η)R=RR d(k d,Η)(15)其中:k c与k d分别为R c与R d定义的旋转轴.R c与R d是已知的,它们是由摄像机外参数与控制器给出的.由式(15),这些已知的数据必然满足R c与R d定义的旋转角相等的条件,与所求的R无关.以下是关于式(15)的解的结构的两个定理[5].
定理1　若R c(k c,Η)R=RR d(k d,Η),则k c=R k d.
方程(15)解的条件k c=R k d的物理意义是,任何满足式(15)的R必须将R d定义的旋转轴k d转到R c所规定的旋转轴k c的位置.
由于满足k c=R k d的解有无穷多个,可见式(15)的解不是惟一的.任意取一个作为式(15)的一个特解,例如,取旋转轴为k=k d×k c,R的旋转角Υ为k d与k c的夹角,显然,当R=R(k,Υ)时,R将k d转到k c,R为式(15)的一个特解.下面的定理,给出了式(15)一般解的形式.
定理2　若R为式(15)的一个特解,则式(15)的所有解R都可以表示为
R=R c(k c,Β)R,(16)其中:k c为R c的旋转轴;Β为任意角.
定理2表明,由式(15)不能惟一地解出R,尚有一个自由度Β待定.式(14)可以写成
(R c-I)t=R t d-t c,(17)其中:R c,t d,t c已知;I为单位对角矩阵;t为待求参数.由上述旋转矩阵的性质可知,
R c-I=U 100
0e-jΗ0
00e-jΗ
U-1UU-1=U
00
0e-jΗ-10
00e-jΗ-1
U-1.
可见,R c-I的特征值有一个是0,当Η不等于0时(即不是单位矩阵时),R c-I为秩2矩阵.由此,即使R能由式(13)解出,式(17)也只给出关于t的3个分量的2个独立的线性方程,所以t 也不能由式(17)惟一确定,尚有一个自由度待定.
在定标过程中,如果控制实验小车作2次运动,则可以得到下面4个关系式
R c1R=RR d1,(18)
R c1t+t c1=R t d1+t,(19)
R c2R=RR d2,(20)
R c2t+t c2=R t d2+t.(21)其中:R c1,t c1,R c2,t c2分别为两次运动的参数,由4次摄像机定标得到的外参数给出;R d1,t d1, R d2,t d2由2次运动时平台旋转的角度和实验小车移动的距离给出.可以由式(18)与(20)联合求出R,再将R代入式(19)与(21)解出t,解的方法如下:
由于式(19)与(21)的解R必须同时满足以下两式(见定理1)
k c1=R k d1,　k c2=R k d2,
45 兰州大学学报(自然科学版) 第37卷
其中:k c1,k c2,k d1,k d2分别为R c1,R c2,R d1,R d2决定的旋转轴方向上的单位向量.由于R 同时将k d1转到k c1,k d2转到k c2,则R 必然将k d1×k d2转到k c1×k c2,将此关系与以上两式同时放在一个矩
阵等式中,可得
(k c1　k c2　k c1×k c2)=R (k d1　k d2　k d1×k d2).当k c1与k c2不互相平行时,上式中的矩阵为满秩矩阵,于是有
R =(k c1　k c2　k c1×k c2)(k d1　k d2　k d1×k d2)-1.
(22)将式(22)中解出的R 代入式(19)与(21),可得到关于t 的3个分量的4个独立的线性方程,任取3个可以解出t ,或用最小二乘法从4个方程中解出t .
3　实验过程及实验结果
实验的基本思路是控制实验小车上的摄像机在不同的位置观察空间一个已知的标定参照物,从而推导R 与t 多次观察结果的关系.
实验系统的组成为:实验小车系统,平台控制器,摄像机镜头,标定参照物,数据处理系统.摄像机固定在平台上,平台又固定在实验小车上.平台可通过平台控制器作旋转运动,实验小车可作平移运动.实验的过程如下:
(1)控制实验小车从A 运动到B ,前后均对摄像机作定标[1,4],求出其外参数.在A 位置,对摄像机定标,求出其外参数变换矩阵A .同理,在B 位置,对摄像机定标,求出其外参数变换矩阵B .由C 1=AB -1可以求得R c1,t c1.由平台旋转的角度Α=8°和实验小车移动的距离(-1.0　1.0　200.0)求出R d1,t d1.
控制实验小车从C 运动到D ,重复上述过程,可以求得R c2,t c2.由平台旋转的角度Α=5°和移动距离(-1.0　-0.5　140.0)求出R d2,t d2.
(2)由式(22)解R .先求出
　k c1=[-0.70773　-0.67847　0.19603]T ,k c2=[0.62718　-0.57867　-0.52133]T ;　k d1=[0　1.00000　0]T , k d2=[0　0　1.00000]T ;
　k c1×k c2=[0.18277　0.05062　0.70338]T ,　k d1×k d2=[1.00000　0　0]T .
将以上各参数代入式(22)解出R
R =　0.62718　0.18277-0.707
73
-0.57867
　0.05062-0.67847-0.52133-0.70338　0.19603
.
(3)将式R 代入式(19)与(21),解出t .
由式(19)与(21)推得t =(R c1-R c2)-1[R (t d1-t d2)-(t c1-t c2)].
求得t 为t =[4.97878　0.76202　313.22905]T .
由式(22)可知,当上述两次运动R d1与R d2的旋转轴不平行时,解R 是惟一的.由式(19)与(21)知,当R c1
与R c2非单位对解矩阵时(运动不是纯平移时),t 也是惟一确定的.
求出R ,t 后,就可以得到车体坐标系与摄像机坐标系之间的变换矩阵M c-t ,即
M c-t =R t 0T 1
=　0.62718　0.18277-0.70773　4.978
78
-0.57867　0.05062-0.67847　0.76202-0.52133　0.70338　0.19603313.229050 0 0 1.00000.55第2期 石广田等:工业机器人运动车体位姿计算方法研究
65 兰州大学学报(自然科学版) 第37卷
4　误差分析
上面求出的变换矩阵有一定的误差,现分析讨论引起误差的主要因素:
(1)标记点或空间目标点投影到图像平面时,识别这些投影点一般靠手工来完成.这样一来,不同的人用鼠标选取就有不同的结果,手工提取某个投影点时,误差最大在3～4个像素.
(2)由于噪声的干扰,在采集的图像中,很难保证标记点的投影有清晰的轮廓.有时当光照条件不好时,投影点处的灰度值会十分接近图像的背景灰度.这个因素在很大程度上也影响着投影点的提取.
(3)图像投影点有时会发生变形,这种变形是非线性的,而计算投影点时进行了局部的线性化.
(4)此外还有设备的误差与计算上的误差.
参　考　文　献
[1]　马颂德,张正友.计算机视觉——计算理论与算法基础[M].北京:科学出版社,1998.
[2]　R ay J.Robo t N avigati on[J].Indu strial Robo t,1994,21(2):130～138.
[3]　孙家广.计算机图形学[M].北京:清华大学出版社,1995.
[4]　杨焕明.立体视觉在轮式移动机器人导航与监测中的应用[D].兰州:兰州铁道学院硕士学位论文,
1999.
[5]　T sai R Y,H uang T S.U n iqueness and esti m ati on of3D mo ti on param eters of rigid bodies w ith cu rved
su rfaces[J].IEEE T ran s,1984,11:13～27.
Research on Ca lcula tion of I ndustr i a l Robot
M ov i ng Position and Posture
S h i Guang tian1,Y u H uan ran1,L i Chong an1,Z hu Chang lin2
(1.D epartm en t of M echan ics,Schoo l of Physical Science and T echno logy,
L anzhou U n iversity,L anzhou,730000,Ch ina;
2.D epartm en t of M echan ical Engineering,L anzhou R ail w ay U n iversity,L anzhou,730070,Ch ina)
Abstract:T he m ethodo logy fo r calcu lati on of indu strial robo t m oving po siti on and po stu re du ring its visual navigati on and m o ti on is p resen ted,the relati on s of car and cam era coo rdinates are discu ssed,and the tran sfo rm m atrix betw een tw o coo rdinate system s is derived.A n exam p le is given in th is p ap er,w h ich m ay gu ide u sers to get m oving p aram eters of indu strial robo t.It is very i m po rtan t that the robo t m oving po siti on and po stu re are calcu lated first fo r its s m oo th visual navigati on.
Key words:robo t;navigati on;po siti on and po stu re;tran sfo rm m atrix。