《机器人定位中的航迹推算》由会员分享，可在线阅读，更多相关《机器人定位中的航迹推算（7页珍藏版）》请在金锄头文库上搜索。
1、航迹推算是一种使用最广泛的定位手段，特别适于短时短距离定位，精度很高。对于长时间运动的，可以应用其他的传感器配合相关的定位算法进行校正。利用陀螺仪和加速度计分别测量出旋转率和加速度，再进行积分，从而可求出走过的距离和航向的变化，进而分析出机器人的位置和姿态。超声波传感器可用于测距，从而探测路标（设置为室内墙壁或天花板） ，计算位置，来纠正陀螺仪和编码器的定位误差。的轮距。因为我们用了陀螺仪可以测出转过的角度，所以没有必要用上面的公式，但上式可用于修正陀螺仪测出的角度值。注：必然要求陀螺安装在机器人不活动的部件上，并且陀螺的安装只能与车体固连在一起。微机械陀螺作为重要的传感器，它的输出信号是一个
2、与转动角速率基本成线性关系的模拟电压值，通过采集其输出的模拟电压值，经过 AD 转换为数字信号，对转换完的信号进行标度变换得到其转动的角速率，再积分即可得到角度值 16。根据以上假设，车体被简化成了一个具有两个平移自由度(纵向和侧向)和一个转动自由度( 横摆) 的单质量刚体。机器人在全局坐标系中的姿态如图 2.3 所示。其中，坐标系 OXY 为全局坐标系， P 点为机器人上的一个参考点，坐标系 为以 P 点为原点的车体固YXO连坐标系， 轴与 X 轴的夹角为 。机器人的姿态（Posture）可以用 P 点在全 局坐标系中的坐标（x，y）和 表示，即可用三维矢量 表示。同时，Tyx),(还可以得
3、到由全局坐标系到车体固连坐标系的坐标旋转矩阵如下：图 2. 1 轮式移动机器人的姿态示意图XYOOxy Pcosin0()i01R本系统在设计时主要参考 DR 航位推算 13，DR 的图解如图 2.5。D(2)D(1)D(0)(X(2),Y(2)(X(1),Y(1)(X(0),Y(0)210图 2. 2 航位推算算法(DR)的原理其原理是以地球表面某点作为当地坐标系的原点，利用里程计输出的距离信息和特定传感器输出的角度信息，计算确定自主车当前的位置。其推导的一般方程为：(2.8)10()()coskiXDi(2.9)niY(2.10)10()()ki其中： , 为初始时刻自主车所在位置， 与
4、为从 i-1 时X()Di刻到 i 时刻自主车行驶距离和方位角。现在航位推算方法很多，但是其本质都是根据传感器得到的数据，或者直接给出方位角的信息，或者推算出方位角的值，然后在代入(2.8)、(2.9)式，求出其相对位置。码盘的距离计算：将两个光电编码器分别安装在车的左右主动轮上，光电编码器同步转动。设车轮直径 D，光电编码器线数（即每转一圈输出的脉冲数）是 P， 时间内t光电编码器输出脉冲数是 N，车轮运行距离是 (左轮运行距离 ，右轮运SLS行距离 )，车轮运行速度是 (左轮运行速度 ，右轮运行速度 )，且RSvLvRv两轮中心距离为 D=2L。那么，/(/)/2()/2/LLRRLRvS
5、tNPDttvNPDtgg通过码盘得到的只是机器小车左右轮的转速，还必须知道两个轮子的周长（或直径） ，才能得到机器小车的行进速度。实际操作中发现，直接测量轮子的半径或直径，误差会很大。有两种方法可以测量轮子的周长：（1）直接法，可以让机器人小车在地上行走，车轮转动一周（或更多） ，再测量机器小车行走的距离；（2）间接法，通过 DSP 工作在仿真模式，让机器小车在地上行走一段距离，测量这段距离，同时在 CCS 中读取码盘的反馈值， （或将程序烧录到DSP 后通过 LED 显示得到码盘的反馈值） 。以上两种方法均可以相对减小测量误差，但推荐采用第二种方法，因为第二种方法可以同时减小或消除码盘测量
6、的误差。不管采用哪一种方法，都应该加大测量距离，同时通过多次测量取平均，才能达到较高的精度。运用上述方法可以得到机器小车左轮前进 1m 的码盘反馈值 和右轮前进0LN1m 的码盘反馈值 （或者左轮的周长 和右轮的周长 ）。0RNLCRC在机器小车行走的过程中，实时采集码盘信息，可以分别得到左右轮的行进速度。而实际系统设计中需要车体的速度，可以将左右轮的速度取平均，得到两轮连线中心的速度即车体的速度 v，这样可以提高精度，更准确的得到车体行进的速度。则（3.1）式变为（4.7）000/()/2(/)2LLRRLRvNN到此，已经实现距离信息的获取。陀螺的角度计算：陀螺的输出信号不是直接与当前的角
7、速率对应，而是输出电压的变化与角速率成比例关系。因此需要一个静态基准作为参比，实际操作中发现V用实验测定的静态基准作为参比电压，短期内精度还可以满足要求。但过一段时间，陀螺的静态基点会漂移，这样会造成很大的误差。所以设计时，实时采集陀螺工作的静态基点，作为参比。由于比赛前机器小车摆放到位到比赛开始还有 1 分钟的时间，可以利用这段时间采集陀螺的静态信号，取平均后作为陀螺当时的静态基点。并且 20 分钟内陀螺的静态基点基本无漂移，所以赛前一分钟内采集的数据可以作为整场比赛的静态基点 。0V机器小车行进过程中不断采集陀螺的输出信号 V，则 ，对应当0前的角速率值 ，其中 k 是陀螺的比例因子。对角
8、速率的积分就可()fxkV以得到角度值，常用积分公式为梯形公式。设已知 时刻的初始角度为 ， 时刻对应的速率值为 ，则可求出a1x()fx时刻的角度 ( )。b2b构造以 ， 为结点的线性插值多项式（4.4）()()()xxafffba则从 时刻到 时刻角度的变化量为：b（4.5）22()()()()1()1)()2baabaxfxdffdxffabfb可得： （4.6） 21 ()()baffb这样，在初始角度 的基础上，每个采样周期都计算一次更新角度 ，这1 2个 相当于下一个周期的初始坐标 ，如此循环，就可以知道任意时刻机器人21的角度变化及姿态。但是机器人在运动的过程中，角速率不断积分
9、，得到的角度会大于 或360者小于 0，这样会给后续的计算和路径规划带来很大麻烦。因此，在实际操作中，将得到的角度规范到0, ）的区间内，即 。360 0,36)要知道机器人的位置坐标，首先需要建立 x-y 二维坐标系。由于根据陀螺和码盘计算的位置坐标和姿态都是相对于车体的起始位置和姿态，即是在车体坐标系（以车体的参考点为坐标原点，x 轴方向与全场坐标系的 x 轴方向大概一致，误差是由摆放的角度误差决定的）中，这样如果比赛策略需求车体的起始摆放位置变化，那么整个坐标系都会有变化，场上关键点的坐标也会变化。这样通用性很差，而且不利于操作。所以建立一个全场坐标系很有必要，它不依赖车体的起始位置和姿
10、态，而是通过改变车体的起始坐标和姿态角来实现不同的摆车需求。图 4. 1 机器人运动原理图建立图 4.8 所示的直角坐标系。机器人从 点出发，经时间 沿圆(,)Axyt弧走到 ，即 ，其中 分别表示机器人A(,)xyA(,xy,在起始位置的横、纵坐标值以及前进方向与横坐标的夹角，逆时针方向为正的方向角， 表示在 时间内机器人的横、纵坐标和角度的增加量，,t表示 点到 点的弧长。于是 可由如下的公式计算得到：S ,xy(4.8)cos()2 2in() s()/cos22xARS(4.9)in() 2si()2 in()/snyARS其中， 表示 点到 点的距离。AA当机器人走直线时 ，则式(4
11、.8),(4.9) 可写成：si()/02(4.10)cosin()2xSy由于时间间隔 很短，线段 的长度近似等于圆弧长度，设 时间内编tAt码盘走过的距离为 ，则 。设 为上次采样的角度， 为当前的角度，则lS12。通过计算 时间内机器人的位置变化量 ，进行累加就可以2t ,xy求出机器人在整个场地上的位置。12 cos() s( co)xAlxl （4.11）（4.12）12 sin() si( in)yyAlyl 这样，在初始坐标(x,y)的基础上，每个采样周期都计算一次更新坐标，这个 相当于下一个周期的初始坐标 ，如此循环，就可以知(,)xy(,)x (,)xy道任意时刻机器人所位置在的坐标。
