第四章 机器人的位置分析4.1 机器人的位置正解方程 4.1.1 引言在这一章中，我们将研究表示各种不同坐标架的齐次变换，并阐述将各坐标架赋给表示操作手的机械连杆系统的方法。
我们将首先规定描述操作手位置和姿态（方位）的各种方法，然后用关节坐标来发展这一描述。
任何操作手都可以认为是由一系列用关节联在一起的杆件组成。
我们在操作手的每一杆件固联上坐标架。
利用齐次变换，我们能够描述这些坐标架之间的相对位置和姿态[3]。
历史上，描述一杆件和下一杆件之间关系的齐次变换称为A 矩阵[1]。
A 矩阵单纯是描述杆件坐标系间相对移动和相对转动的齐次变换。
1A 描述第一杆的位置和姿态。
2A 描述第二杆相对与第一杆的位置和姿态。
于是，第二杆在基座坐标架中的位置和姿态用矩阵乘积给出如下：=212T A A （4-1） 同样，3A 表示通过第二杆来描述第三杆，且3=312T A A A （4-2）这些A 矩阵的乘积一直称为T 矩阵，而且如果前置上标为0则略去。
给出一个六杆操作手，我们得到36=61245T A A A A A A （4-3）一个六杆操作手具有6个自由度，每杆一个，且能在其活动范围内有任意的位置和姿态。
三个自由度用来规定位置，二另外三个用来规定姿态。
6T 描述操作手的位置和姿态。
这可用图4-1所示的手来思考。
我们将在指尖间的正中设置坐标架的原点，这一原点用向量p 来描述。
描述手部姿态的三个单位向量定向如下。
向量Z 设在手部接近物体的方向上，称为接近向量a 。
另一称为姿态向量o 的y 向量设在从一指尖到另一指尖的方向以表明手部的姿态。
最后一个称为法向向量的n 向量，与a 、o 形成一组右旋向量因而可用向量的叉乘表示为：=⨯n o a 于是变换6T 具有如图4.1所示元素1⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎣⎦6z zz z T x x x x yy y y n o a p n o a p n o a p （4-4）图4-1 o ,a ,n 和p 向量4.1.2 姿态的规定给16个元素一一赋值，6T 就被完全规定。
这16个元素中，仅12个元素有任意意义。
低行由三个0和一个1组成。
左方列向量，是o 和a 列向量的叉乘。
余下的9个表示o 、a 和p 三个列向量。
表示操作手能够到达预期位置的p 值没有限制，而o 和a 则必须是正交单位向量，即是⋅=o o 1 （4-5） ⋅=a a 1 （4-6） ⋅=o a 0 （4-7）除末端夹持器于坐标轴方向重合的简单情况外，对o 和a 的这些限制，使得难以去确定向量的分量。
如果有o 和a 遇到难以确定向量分量的任何麻烦，可把向量改成满足下述条件的形式。
是a 为单位值←aa a（4-8） 作垂直于o 和a←⨯n o a （4-9）在由o 和a 形成的平面上转动o ，使它同时垂直于n 和a←⨯o a n （4-10）且使o 为单位值，←oo o（4-11）我们也可以用在第三章中研究的广义旋转矩阵Rot(k θ),来规定操作手末端的姿态为绕k 轴转动θ角。
不幸的是，为获得某一预期姿态的旋转轴不是直觉明显的。
4.1.3 欧拉角经常借助绕x y ，和z 轴的连续转动来规定姿态。
欧拉角通过绕z 轴转动φ，然后绕新的y 轴y '转动θ，最后绕新的z 轴z ''转动ψ来描述任何可能的姿态（见图4-2）。
z y z φθψφθψ=Euler(,,)Rot(,)Rot(,)Rot(,) （4-12） 在整个连续转动过程中，转动的次序是重要的。
注意这一连续转动可以用相反的转动次序来阐明：首先绕z 轴转动ψ，接着绕基础y 轴转动θ，最后再次绕基础z 轴转动φ（见图4-3）。
欧拉变换φθψEuler(,,)能够通过三个旋转矩阵联乘给予赋值z y z φθψφθψ=Euler(,,)Rot(,)Rot(,)Rot(,) （4-13）cos 0sin 00100(,,)(,)sin 0cos 00001cos sin 00sin cos 0000100001z θθφθψφθθψψψψ⎡⎤⎢⎥⎢⎥=⎢⎥-⎢⎥⎣⎦-⎡⎤⎢⎥⎢⎥⨯⎢⎥⎢⎥⎣⎦Euler Rot （4-14）cos sin 00sin cos 00(,,)00100001cos cos cos sin sin 0sin cos 00sin cos sin sin cos 00001φφφφφθψθψθψθψψθψθψθ-⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦-⎡⎤⎢⎥⎢⎥⨯⎢⎥-⎢⎥⎣⎦Euler （4-15）cos cos cos sin sin sin cos cos cos sin (,,)sin cos 0cos cos sin sin cos cos sin 0sin cos sin cos cos sin sin 0sin sin cos 0001φθψφψφθψφψφθψθψφθψφψφθφθψφψφθθψθ-⎡⎢+⎢=⎢-⎢⎣--⎤⎥-+⎥⎥⎥⎦Euler（4-16）4.1.4 滚转，俯仰和侧摆另一经常使用的旋转组合是滚转，俯仰和侧摆。
如果我们想像一只沿z 轴行驶的船，则滚转相应于绕z 轴转动φ，俯仰相应于绕y 轴转动θ，而侧摆相应于绕x 轴转动ψ，（见图4-4）。
操作手末端夹持器的这些转动见图4-5。
我们将规定转动顺序为RPY(,,)Rot(,)Rot(,)Rot(,)z y x φθψφθψ= （4-17）图4-2 欧拉角 图4-3 在基坐标中阐明的欧拉角那就是先绕x 轴转动ψ，再绕y 轴转动θ，最后绕z 轴转动φ。
这一变换如下cos 0sin 00100(,,)(,)sin 0cos 00001RPY Rot z θθφθψφθθ⎡⎤⎢⎥⎢⎥=⎢⎥-⎢⎥⎣⎦10000cos sin 00sin cos 00001 ψψψψ⎡⎤⎢⎥-⎢⎥⨯⎢⎥⎢⎥⎣⎦（4-18） cos sin 00sin cos 00(,,)00100001cos sin sin sin cos 00cos sin 0sin cos sin cos cos 00001RPY φφφφφθψθθψθψψψθθψθψ-⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦⎡⎤⎢⎥-⎢⎥⨯⎢⎥-⎢⎥⎣⎦（4-19）cos cos cos sin sin sin cos sin cos sin sin sin cos cos (,,)sin cos sin 00cos sin cos sin sin 0sin sin cos cos sin 0cos cos 001RPYφθφθψφψφθφθψφψφθψθθψφθψφψφθψφψθψ-⎡⎢+⎢=⎢-⎢⎣+⎤⎥-⎥⎥⎥⎦（4-20）4.1.5 位置的规定一旦姿态被规定，将其乘以相应于向量P 的平移变换，就可以把手定位在基座坐标中，10001000100016T x y z p p p ⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦某一姿态变换 （4-21）4.1.3 圆柱坐标但是，我们可能希望用圆柱坐标来规定手的位置。
这相应于先沿x 轴平移r ，接着绕z轴转动α，最后沿z 轴平移z （见图4-6）。
0000Cyl(,,)Trans(,,)Rot(,)Trans(,,)z r z z r αα=cos sin 00100sin cos 000100(,,)(0,0,)0010001000010001Cyl Trans r z r z ααααα-⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=⨯⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦（4-22） 1000cos sin 0cos 0100sin cos 0sin (,,)00100100001001Cyl r r z r z ααααααα-⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦（4-23） cos sin 0cos sin cos 0sin (,,)0010001Cyl r r z r z ααααααα-⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦（4-24） 如果我们将这一变换后乘象公式（4-21）那样一个姿态变换，则手的姿态将相对于基座坐标绕z 轴转动α。
如果按没有转动的基座坐标来规定姿态，则我们要将（4-24）绕它的绕z 轴转动α-角。
cos sin 0cos cos()sin()00sin cos 0sin sin()cos()00(,,)001001000010001Cyl r r z r z ααααααααααα----⎡⎤⎡⎤⎢⎥⎢⎥--⎢⎥⎢⎥=⨯⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦（4-25） cos sin 0cos cos sin 00sin cos 0sin sin cos 00(,,)0010010001001Cyl r r z r z ααααααααααα-⎡⎤⎡⎤⎢⎥⎢⎥-⎢⎥⎢⎥=⨯⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦（4-26） 100cos 010sin (,,)0010001Cyl r r z r z ααα⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦（4-27） 这就是我们要用来解释(,,)Cyl z r α的形式。
4.1.7 球 坐 标最后，我们将考虑用球坐标规定位置向量的方法。
这方法相应于先沿z 轴平移r ，接着绕y 轴转动β，最后绕z 轴转动α（见图4-7）。
(,,)(,)(,)(0,0,)Sph Rot Rot Trans r z y r αβαβ= （4-28）cos 0sin 0100001000100(,,)(,)sin 0cos 000100010001Sph Rot r z r ββαβαββ⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=⨯⎢⎥⎢⎥-⎢⎥⎢⎥⎣⎦⎣⎦（4-29） cos 0sin 0cos 0sin sin 01000100(,,)sin 0cos 0sin 0cos cos 00010001Sph r r βββββαββββββ-⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=⨯⎢⎥⎢⎥-⎢⎥⎢⎥⎣⎦⎣⎦（4-30）ycos cos sin cos sin cos sin sin cos cos sin sin sin sin (,,)sin 0cos cos 0001Sph αβααβαβαβααβαβαββββ-⎡⎤⎢⎥⎢⎥=⎢⎥-⎢⎥⎣⎦r r r r （4-31） 再次，如果我们不希望按这一转动后坐标架来表示姿态，我们必须后乘(,)Rot y β-和(,)Rot z α-(,,)(,)(,)(0,0,)(,)(,)Sph Rot Rot Trans Rot Rot r z y r y z αβαββα=-- （4-32）100cos sin 010sin sin (,,)001cos 0001Sph r r r r αβαβαββ⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦（4-33） 4.1.8 6T 的规定6T 可以用一个转动和一个移动的多种方式来规定。
