CN103307968A - 机器人承载平台姿态的检测方法 - Google Patents

Abstract

本发明公开了一种机器人承载平台姿态的检测方法，用于解决现有检测方法检测精度低的技术问题。技术方案是在机器人承载平台的初始化位置设定一个世界坐标系作为参考坐标系，通过球栅尺以及球栅尺测量触头配合检测动坐标系相对世界坐标系的二维坐标（x，y），通过磁栅尺与静磁珊源配合，通过磁栅尺上显示的当前磁珊源的高度反求机器人承载平台的z坐标和绕x、y两个坐标轴的旋转角度变化值，通过采集磁珊源数据采集端数据获得机器人承载平台围绕x、y轴旋转的角度；通过叉乘世界坐标系下A1、C1点坐标值，计算待测平面法向量N_Z。然后叉乘N_Z和世界坐标系下O1A1即得O1C1方向，叉乘N_Z和O1C1即得O1A1方向。由于引入球栅尺、磁栅尺等非接触式测量，提高了测量精度。

Description

机器人承载平台姿态的检测方法

技术领域

本发明涉及一种检测方法，特别是涉及一种机器人承载平台姿态的检测方法。

背景技术

在现代国防和民用生产制造技术中，常常需要把一个装载测量设备的承载平台精确地调整到要求的指定位置，以提高系统的工作性能，满足生产加工零件的精度要求。如用于给某个指定区域制造人工降雨的发射固体碘化银的火箭平台，还有飞航导弹、战术地空导弹以及战斗火炮等的发射平台等，姿态的找正对检测系统本身的性能有着很大的影响。因此，承载平台的调平找正，尤其是高精度承载平台调平方法的研究就显得刻不容缓。

随着科学技术的进步，特别是各种光栅式、磁珊式、球珊式等光测量方法与传感器技术的快速发展，测量手段更加多样化和精密化，相比于传统常用的检测方法，四点调平算法，该方法虽然较为可靠，但存在静不定的问题，且操作复杂繁琐难以实现。针对该情况，本文提出一种基于三点平面法进行姿态的找正与调整，该方法控制精度更高，且实际操作简单易行。

文献“专利公开号是CN101957175A的中国发明专利”公开了一种基于三点微平面式法向检测方法，该方法采用环形均布于测量机头部的三个激光传感器采集数据，根据激光点形成的微平面的大小控制精度。该方法的不足之处在于精度的提高依赖于微平面的无限缩小，而微平面缩小的有限性局限了该方法的精度。因为该方法中所用的激光传感器不仅价格过于昂贵，且不能适应油污、粉尘这样的工作环境。

发明内容

为了克服现有检测方法检测精度低的不足，本发明提供一种机器人承载平台姿态的检测方法。该方法在机器人承载平台的初始化位置设定一个世界坐标系作为参考坐标系，通过球栅尺以及球栅尺测量触头配合检测动坐标系相对世界坐标系的二维坐标（x，y），通过磁栅尺与静磁珊源配合，通过磁栅尺上显示的当前磁珊源的高度反求机器人承载平台的z坐标和绕x、y两个坐标轴的旋转角度变化值，通过采集磁珊源数据采集端数据获得机器人承载平台围绕x轴旋转的角度，通过磁珊源数据采集端数据获得机器人承载平台后仰的角度，及围绕y轴旋转的角度；绕水平面两个轴旋转的角度值经过空间姿态变换运算求知在垂直地面方向位移的变化值。采集球珊尺测量触头端子数据，即为在世界坐标系中O运动时间t后到O1位置水平面二维坐标值的变化；计算移动后空间点A1、C1在世界坐标系下的坐标；通过叉乘世界坐标系下A1、C1点坐标值，计算待测平面法向量N_Z。然后叉乘N_Z和世界坐标系下O1A1即得O1C1方向，叉乘N_Z和O1C1即得O1A1方向。由于引入球栅尺、磁栅尺等非接触式测量，可以提高测量精度。

本发明解决其技术问题所采用的技术方案是：一种机器人承载平台姿态的检测方法，其特点是包括以下步骤：

第一步，在机器人承载平台的初始化位置设置一个世界坐标系作为参考坐标系，在机器人承载平台上任选三个点组成待测平面，该待测平面随着机器人承载平台的移动而发生姿态的变化，即为动坐标系。设A、B、C分别是在机器人承载平台任意取定的三个点，过C点做AB的垂线，垂足为O，以O为坐标原点，OA、OC分别为x轴，z轴，坐标系Oxyz即为初始化的动坐标系。机器人承载平台移动后的姿态用O1x1y1z1来表示。

第二步，在搭载机器人承载平台的滑轨上安装球栅尺4，在滑轨与承载平台配合处安装球栅尺测量触头3，球栅尺4与球栅尺测量触头3配合检测动坐标系相对世界坐标系的二维坐标（x，y）；垂直于水平面且垂直滑轨一侧和垂直于水平面且平行滑轨一侧各安装一个磁栅尺1，机器人承载平台中心安装静磁珊源5，通过磁栅尺1上显示的当前磁珊源5的高度反求机器人承载平台此刻的z坐标和绕x、y两个坐标轴的旋转角度变化值。

第三步，采集平行滑轨方向安装的磁珊源5数据采集端数据获得机器人承载平台围绕x轴旋转的角度，采集垂直滑轨放置的磁珊源5数据采集端数据获得机器人承载平台后仰的角度，及围绕y轴旋转的角度；绕水平面两个轴旋转的角度值经过空间姿态变换运算求出在垂直地面方向位移的变化值。采集球珊尺测量触头3端子数据，即为在世界坐标系中O运动时间t后到O1位置水平面二维坐标值的变化。

第四步，通过三维坐标测量仪获取A、C点在世界坐标系下的位置坐标，该坐标即为A1、C1在动坐标系下的坐标，应用空间复合变换和欧拉坐标转换公式，使用求解O1在世界坐标系中的三维坐标的计算方法，计算出A1、C1在世界坐标系下的坐标。坐标转换公式如下：

$P_i=\mathrm{RP}_j^i{}_j+P_i^{\mathrm{oj}}$

$R_i^j=R(\mathrm{OZ},\mathrm{\α})*R(\mathrm{OC},\mathrm{\β})*R(\mathrm{OA},\mathrm{\γ})$

$=\left[\begin{array}{ccc}\cos \mathrm{\α}& -\sin \mathrm{\α}& 0\\ \sin \mathrm{\α}& \cos \mathrm{\α}& 0\\ 0& 0& 1\end{array}\right]*\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\β}& -\sin \mathrm{\β}\\ 0& \sin \mathrm{\β}& \cos \mathrm{\β}\end{array}\right]*\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& -\sin \mathrm{\γ}\\ 0& 1& 0\\ \sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]$

机器人承载平台的姿态变化，即三维平动值和每个平面法向量方向的变化，以上α、β、γ即为法向量姿态的变化量。

第五步，将第四步计算的世界坐标系下A1、C1点坐标叉乘，设A1、C1在世界坐标系下的坐标分别为P_a1、P_c1，P_a1x表示三维坐标中x分量，按照以下公式计算待测平面法向量，

$\stackrel{\→}{P_{\mathrm{oj}}{P}_{\mathrm{cl}}}=(P_{\mathrm{clx}}-P_{\mathrm{ojx}},P_{\mathrm{cly}}-P_{\mathrm{ojy}},P_{\mathrm{clz}}-P_{\mathrm{ojz}})$

$\stackrel{\→}{P_{\mathrm{oj}}{P}_{\mathrm{al}}}=(P_{\mathrm{alx}}-P_{\mathrm{ojx}},P_{\mathrm{aly}}-P_{\mathrm{ojy}},P_{\mathrm{alz}}-P_{\mathrm{ojz}})$

$N_z=\stackrel{\→}{P_{\mathrm{oj}}{P}_{\mathrm{cl}}}\×\stackrel{\→}{P_{\mathrm{oj}}{P}_{\mathrm{al}}}$

然后叉乘N_Z和

即得O1C1方向，叉乘N_Z和

即得O1A1方向。计算结果带入第四步反求机器人承载平台姿态变化中各参数变化量。

本发明的有益效果是：由于该方法在机器人承载平台的初始化位置设定一个世界坐标系作为参考坐标系，通过球栅尺以及球栅尺测量触头配合检测动坐标系相对世界坐标系的二维坐标（x，y），通过磁栅尺与静磁珊源配合，通过磁栅尺上显示的当前磁珊源的高度反求机器人承载平台的z坐标和绕x、y两个坐标轴的旋转角度变化值，通过采集磁珊源数据采集端数据获得机器人承载平台围绕x轴旋转的角度，通过磁珊源数据采集端数据获得机器人承载平台后仰的角度，及围绕y轴旋转的角度；绕水平面两个轴旋转的角度值经过空间姿态变换运算求知在垂直地面方向位移的变化值。采集球珊尺测量触头端子数据，即为在世界坐标系中O运动时间t后到O1位置水平面二维坐标值的变化；计算移动后空间点A1、C1在世界坐标系下的坐标；通过叉乘世界坐标系下A1、C1点坐标值，计算待测平面法向量N_Z。然后叉乘N_Z和世界坐标系下O1A1即得O1C1方向，叉乘N_Z和O1C1即得O1A1方向。由于引入球栅尺、磁栅尺等非接触式测量，提高了测量精度。

下面结合附图和实施例对本发明作详细说明。

附图说明

图1是本发明方法中机器人承载平台在世界坐标系的位置示意图。

图2是本发明方法中机器人承载平台移动后在世界坐标系的位置示意图。

图3是本发明机器人承载平台姿态的检测方法用测量工具安装位置示意图。

图4是本发明机器人承载平台姿态的检测方法用动坐标系相对于世界坐标系的变换原理图。

图中，1-磁栅尺，2-机器人承载平台，3-球栅尺测量触头，4-球栅尺，5-磁栅源。

具体实施方式

以下实施例参照图1～4。

机器人承载平台姿态检测方法是基于对微小形变进行放大的思想进行测量，而且球栅尺、磁栅尺精度高、测量方便、快捷。以下结合具体附图对该方法做进一步详细说明。

①进行空间坐标系的建立与初始化。在机器人承载平台的初始化位置处人为的规定一个世界坐标系作为参考坐标系，本文假定选在搭载机器人承载平台的滑轨的右下角位置，在机器人承载平台上任选三个点组成待测平面，该平面随着机器人承载平台的移动而发生姿态的变化，即为动坐标系。设A、B、C分别是在机器人承载平台任意取定的三个点，过C点做AB的垂线，垂足为O，以O为坐标原点，OA、OC分别为x轴，z轴，坐标系Oxyz即为初始化的动坐标系。因为动坐标系是固化在机器人承载平台上，随着机器人的位姿变化而变化，所以机器人承载平台移动后的姿态可以用O1x1y1z1来表示，造成机器人承载平台位姿变化的误差包括两部分：平动误差和旋转误差。旋转误差是由机器人承载平台可能会出现的俯仰，侧翻等姿态导致，平动误差是机器人承载平台在水平面上位置的偏移。

②建立模型。在搭载机器人承载平台的滑轨上安装球栅尺4，在滑轨与承载平台配合处安装球栅尺测量触头3，球栅尺4与球栅尺测量触头3配合检测动坐标系相对世界坐标系的二维坐标（x，y）；垂直于水平面且垂直滑轨一侧和垂直于水平面且平行滑轨一侧各安装一个磁栅尺1，机器人承载平台中心安装静磁珊源5，通过磁栅尺1上显示的当前静磁珊源5的高度反求机器人承载平台此刻的z坐标和绕x、y两个坐标轴的旋转角度变化值。

③采集数据。采集平行滑轨方向安装的磁珊源5数据采集端数据获得机器人承载平台围绕x轴旋转的角度，采集垂直滑轨放置的磁珊源5数据采集端数据获得机器人承载平台后仰的角度，及围绕y轴旋转的角度；绕水平面两个轴旋转的角度值经过空间姿态变换运算求出在垂直地面方向位移的变化值。采集球珊尺测量触头3端子数据，即为在世界坐标系中O运动时间t后到O1位置水平面二维坐标值的变化。

④计算移动后空间点A1、C1在世界坐标系下的坐标。首先，A、C点在世界坐标系下的位置坐标可以通过三维坐标测量仪获取，该坐标即为A1、C1在动坐标系下的坐标，应用空间复合变换和欧拉坐标转换公式，同理使用求解O1在世界坐标系中的三维坐标的计算方法，计算出A1、C1在世界坐标系下的坐标。

坐标转换公式如下：

$P_i=\mathrm{RP}_j^i{}_j+P_i^{\mathrm{oj}}$

$R_i^j=R(\mathrm{OZ},\mathrm{\α})*R(\mathrm{OC},\mathrm{\β})*R(\mathrm{OA},\mathrm{\γ})$

$=\left[\begin{array}{ccc}\cos \mathrm{\α}& -\sin \mathrm{\α}& 0\\ \sin \mathrm{\α}& \cos \mathrm{\α}& 0\\ 0& 0& 1\end{array}\right]*\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\β}& -\sin \mathrm{\β}\\ 0& \sin \mathrm{\β}& \cos \mathrm{\β}\end{array}\right]*\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& -\sin \mathrm{\γ}\\ 0& 1& 0\\ \sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]$

机器人承载平台的姿态变化，即三维平动值和每个平面法向量方向的变化，以上α、β、γ即为法向量姿态的变化量。

基于空间复合变换和欧拉坐标变换公式，进行动坐标系相对世界坐标系各个变化量的计算，在动坐标系内取一点P，在动坐标系中P点位置通过三维测量仪获取，求解P在世界坐标系中的位置坐标步骤是：引入中间坐标系C，其满足与坐标系1同姿态，与动坐标系同位置。姿态的旋转变换式为：

先旋转后平移变换式为：依据这个坐标变换公式，求取A1、C1点在世界坐标下的三维坐标值，然后根据

求取OjA1C1平面的法向量，重复以上方法，叉乘N_Z和

向量，即为x1轴方向；叉乘N_Z和

向量，即为y1方向。

⑤计算法向量。将步骤④计算的世界坐标系下A1、C1点坐标叉乘，设A1、C1在世界坐标系下的坐标分别为P_a1、P_c1，P_a1x表示三维坐标中x分量，由此计算待测平面法向量，公式如下：

$\stackrel{\→}{P_{\mathrm{oj}}{P}_{\mathrm{cl}}}=(P_{\mathrm{clx}}-P_{\mathrm{ojx}},P_{\mathrm{cly}}-P_{\mathrm{ojy}},P_{\mathrm{clz}}-P_{\mathrm{ojz}})$

$\stackrel{\→}{P_{\mathrm{oj}}{P}_{\mathrm{al}}}=(P_{\mathrm{alx}}-P_{\mathrm{ojx}},P_{\mathrm{aly}}-P_{\mathrm{ojy}},P_{\mathrm{alz}}-P_{\mathrm{ojz}})$

$N_z=\stackrel{\→}{P_{\mathrm{oj}}{P}_{\mathrm{cl}}}\×\stackrel{\→}{P_{\mathrm{oj}}{P}_{\mathrm{al}}}$

然后叉乘N_Z和

即得O1C1方向，叉乘N_Z和

即得O1A1方向。计算结果带入步骤④反求机器人承载平台姿态变化中各参数变化量。该方法测得的法向量可用于法向误差校正、法向姿态调整等测量领域。该方法利用了非接触式测量，测量精度高，方便等特点。

Claims (1)

1.一种机器人承载平台姿态的检测方法，其特征在于包括以下步骤：

第一步，在机器人承载平台的初始化位置设置一个世界坐标系作为参考坐标系，在机器人承载平台上任选三个点组成待测平面，该待测平面随着机器人承载平台的移动而发生姿态的变化，即为动坐标系；设A、B、C分别是在机器人承载平台任意取定的三个点，过C点做AB的垂线，垂足为O，以O为坐标原点，OA、OC分别为x轴，z轴，坐标系Oxyz即为初始化的动坐标系；机器人承载平台移动后的姿态用O1x1y1z1来表示；

第二步，在搭载机器人承载平台的滑轨上安装球栅尺4，在滑轨与承载平台配合处安装球栅尺测量触头3，球栅尺4与球栅尺测量触头3配合检测动坐标系相对世界坐标系的二维坐标（x，y）；垂直于水平面且垂直滑轨一侧和垂直于水平面且平行滑轨一侧各安装一个磁栅尺1，机器人承载平台中心安装静磁珊源5，通过磁栅尺1上显示的当前磁珊源5的高度反求机器人承载平台此刻的z坐标和绕x、y两个坐标轴的旋转角度变化值；

第三步，采集平行滑轨方向安装的磁珊源5数据采集端数据获得机器人承载平台围绕x轴旋转的角度，采集垂直滑轨放置的磁珊源5数据采集端数据获得机器人承载平台后仰的角度，及围绕y轴旋转的角度；绕水平面两个轴旋转的角度值经过空间姿态变换运算求出在垂直地面方向位移的变化值；采集球珊尺测量触头3端子数据，即为在世界坐标系中O运动时间t后到O1位置水平面二维坐标值的变化；

第四步，通过三维坐标测量仪获取A、C点在世界坐标系下的位置坐标，该坐标即为A1、C1在动坐标系下的坐标，应用空间复合变换和欧拉坐标转换公式，使用求解O1在世界坐标系中的三维坐标的计算方法，计算出A1、C1在世界坐标系下的坐标；坐标转换公式如下：

$P_i=\mathrm{RP}_j^i{}_j+P_i^{\mathrm{oj}}$

$R_i^j=R(\mathrm{OZ},\mathrm{\α})*R(\mathrm{OC},\mathrm{\β})*R(\mathrm{OA},\mathrm{\γ})$

$=\left[\begin{array}{ccc}\cos \mathrm{\α}& -\sin \mathrm{\α}& 0\\ \sin \mathrm{\α}& \cos \mathrm{\α}& 0\\ 0& 0& 1\end{array}\right]*\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\β}& -\sin \mathrm{\β}\\ 0& \sin \mathrm{\β}& \cos \mathrm{\β}\end{array}\right]*\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& -\sin \mathrm{\γ}\\ 0& 1& 0\\ \sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]$

机器人承载平台的姿态变化，即三维平动值和每个平面法向量方向的变化，以上α、β、γ即为法向量姿态的变化量；

第五步，将第四步计算的世界坐标系下A1、C1点坐标叉乘，设A1、C1在世界坐标系下的坐标分别为P_a1、P_c1，P_a1x表示三维坐标中x分量，按照以下公式计算待测平面法向量，

$\stackrel{\→}{P_{\mathrm{oj}}{P}_{\mathrm{cl}}}=(P_{\mathrm{clx}}-P_{\mathrm{ojx}},P_{\mathrm{cly}}-P_{\mathrm{ojy}},P_{\mathrm{clz}}-P_{\mathrm{ojz}})$

$\stackrel{\→}{P_{\mathrm{oj}}{P}_{\mathrm{al}}}=(P_{\mathrm{alx}}-P_{\mathrm{ojx}},P_{\mathrm{aly}}-P_{\mathrm{ojy}},P_{\mathrm{alz}}-P_{\mathrm{ojz}})$

$N_z=\stackrel{\→}{P_{\mathrm{oj}}{P}_{\mathrm{cl}}}\×\stackrel{\→}{P_{\mathrm{oj}}{P}_{\mathrm{al}}}$

然后叉乘N_Z和

即得O1C1方向，叉乘N_Z和

即得O1A1方向；计算结果带入第四步反求机器人承载平台姿态变化中各参数变化量。

Images