一种基于距离测量求解球关节三自由度姿态的装置及方法

摘要

本发明公开了一种基于距离测量求解球关节三自由度姿态的装置及方法，属于多自由度系统姿态测量领域。该装置包括一个球壳和至少三个距离传感器；所述球壳用于安装至待测球关节外，与待测球关节的动子同心固连，其姿态会随着球关节的转轴姿态的改变而同步改变；所述球壳的半径是变化的，且与球关节运动姿态之间存在一预设的函数映射关系；所述至少三个距离传感器的位置不同，且均指向球壳的球心。基于上述装置，通过球壳外形的设计，使得球壳的半径与球关节运动姿态之间存在一定的函数映射关系，利用各距离传感器测得的到球壳的距离计算出至少三个球壳半径数据，进而根据球壳半径与球关节运动姿态之间的函数映射关系来反解出待测球关节的运动姿态。

说明书

技术领域

本发明属于多自由度系统姿态测量领域，更具体地，涉及一种能够通过距离测量求解球关节多自由度姿态的方法。

背景技术

现多自由度的运动在很多领域的研究中都是必要的，在各种多自由度执行器的研究与使用中，多自由度执行器的运动姿态检测一直是一个重点和难点。由于多个自由度耦合在一起，导致姿态测量难度大，又因为运动姿态检测是实现多自由度执行器精确控制的最重要的一环，因此运动姿态的检测对于研究和应用来说至关重要。

多自由度执行器有很多种，比较具有代表性是球关节结构，现阶段球关节的姿态检测方法有两种：接触式和非接触式。接触式姿态检测方法代表性的结构由两个半圆的滑轨支架、一个滑块与其上的三个旋转编码器组成。三个编码器的输出间接得出转子在各方向上的偏转角，从而实现对电动机转子方位的跟踪检测，这种测量方法所采用的机械联接装置联接转子和旋转编码器，会引入无法忽略的惯性力和摩擦，会极大的影响检测精度。非接触式姿态检测方法代表性的有基于光学和基于磁场两种方法。光学非接触式姿态检测方法通常由光线发射单元，光线接收单元组成，通常发射单元固联在动子上，随着动子运动，接收单元根据接收光线的变化来反解运动姿态，该类方法所需设备结构比较大，同时对环境光线有一定要求，并且运算量很大。基于磁场的非接触式姿态检测方法是通过测量动子磁铁在运动过程中产生的变化的磁场来反推运动姿态，该方法对磁铁磁化精度要求较高，磁场标定较为复杂。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于距离测量求解球关节三自由度姿态的装置及方法，其目的在于，通过设计不均匀的球壳，通过距离传感器测量到球壳的距离，转换为球关节的运动姿态，大大减少了误差项，且不需要借助磁场，由此解决现有技术复杂且精度不高的技术问题。

为实现上述目的，按照本发明的一个方面，提供了一种基于距离测量求解球关节三自由度姿态的装置，包括一个球壳和至少三个距离传感器；

所述球壳用于安装至待测球关节外，与待测球关节的动子同心固连，其姿态会随着球关节的转轴姿态的改变而同步改变；所述球壳的半径是变化的，且与球关节运动姿态之间存在一预设的函数映射关系；

所述至少三个距离传感器的位置不同，且均指向球壳的球心。

为了解决上述问题，本发明还提供了基于上述装置求解球关节三自由度姿态的方法，通过球壳外形的设计，使得球壳的半径与球关节运动姿态之间存在一定的函数映射关系，利用各距离传感器测得的到球壳的距离计算出至少三个球壳半径数据，进而根据球壳半径与球关节运动姿态之间的函数映射关系来反解出待测球关节的运动姿态。

进一步地，求解过程如下：

球关节的运动姿态用欧拉角(α,β,γ)表示，建立定子坐标系XYZ、与转轴固连的动子坐标系xyz，坐标系XYZ与xyz初始位置重合，两坐标原点O与球壳球心重合；

设球壳的外壳半径满足r＝f(θ,φ)，(θ,φ,r)代表球壳上一点在动子坐标系下的球坐标表示；

设(Θ,Φ,R)和(θ_s,φ_s,R)分别是距离传感器的位置在定子坐标系和动子坐标系下的球坐标表示，对应在定子坐标系和动子坐标系下的直角坐标分别表示为：

S＝[RS_ΘC_Φ>ΘS_Φ>Θ]^T

s＝[RS_θsC_φs>θsS_φs>θ]^T

其中，S是距离传感器在定子坐标系下的直角坐标表示，

s是距离传感器在动子坐标下的直角坐标表示，满足如下关系，其中R为旋转矩阵：

s＝R^TS

则由以上关系式可以解出θ_s,φ_s与α,β,γ之间的函数关系，记为：

θ_s＝g(α,β,γ)

φ_s＝h(α,β,γ)

则由以上关系式可以得到传感器测量值d的表达式：

d＝R-r＝R-f(θ_s,φ_s)＝R-f(g(α,β,γ),h(α,β,γ))

由此便建立了传感器测量值d与运动姿态(α,β,γ)之间的函数映射关系；三个摆放在不同位置的传感器可以得到三个该函数映射关系，从而唯一解出α,β,γ。

进一步地，根据传感器测量值d与运动姿态(α,β,γ)之间的函数映射关系求解α,β,γ的过程如下：从运动起始位置开始，将上一刻(α,β,γ)的值作为当前时刻的迭代初值，应用梯度下降法，解出当前时刻(α,β,γ)的精确数值解。

总体而言，本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

1、通过自主设计球壳形状，利用距离传感器测距，直接建立传感器测量值d与运动姿态(α,β,γ)之间的函数映射关系，从而直接求解出运动姿态，快捷方便，且无需复杂的装置设置。相比于现有技术，无需考虑惯性力、摩擦、磁场，大大提高测量精度，且降低成本。

2、由于球壳形状函数可以自定义，最终均可转换为传感器测量值d与运动姿态(α,β,γ)之间的函数映射关系，因此适应于各种测量环境。

附图说明

图1是整体测量方案的结构图。

图中，1-球关节，2-球壳，3-距离传感器。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

图1显示的是球关节姿态方案的结构图，如图中所示，该方案由球壳、球关节、距离传感器组成。球壳的外壳半径满足r＝f(θ,φ)，(θ,φ,r)代表球壳上一点在动子坐标系下的球坐标表示。

球关节的运动姿态用欧拉角(α,β,γ)表示，如图1所示，图中坐标系XYZ是定子坐标系，坐标系xyz是与转轴固连的动子坐标系，坐标系XYZ与xyz初始位置重合,两坐标原点O与球壳球心重合。转轴先绕着x轴旋转α角，然后绕着y轴旋转β角，最后绕着z轴旋转γ角。

本发明中所设计的球壳的半径是变化，并非一个均匀的球形。通过球壳外形的设计，使得球壳的半径与球关节运动姿态之间存在一定的函数映射关系，利用距离传感器测得的到球壳的距离来计算出球壳的半径，根据球壳半径与球关节运动姿态之间的函数映射关系来反解出运动姿态。距离测量很容易实现，并且可以达到高精度。因此，用此方法可以实现低成本，高精度和快速测量。图1显示的是整体测量方案的结构图，图中所涉及到的传感器为距离传感器，指向球心。图1中的(Θ,Φ,R)和(θ,φ,R)分别是传感器的位置在定子坐标系和动子坐标系下的球坐标表示。球壳与球关节动子同心固连，其姿态会随着转轴姿态的改变而改变。

传感器位置固定，其在定子坐标系下的坐标已知，球坐标表示为(Θ,Φ,R)，且传感器指向球心O,其位置在动子坐标系下可表示为(θ_s,φ_s,R)，对应在定子坐标系和动子坐标系下的直角坐标分别表示为：

S＝[RS_ΘC_Φ>ΘS_Φ>Θ]^T

s＝[RS_θsC_φs>θsS_φs>θ]^T

其中，S是传感器在定子坐标系下的直角坐标表示，s是传感器在动子坐标下的直角坐标表示，满足如下关系，其中R为旋转矩阵：

s＝R^TS

则由以上关系式可以解出θ_s,φ_s与α,β,γ之间的函数关系，可以写为：

θ_s＝g(α,β,γ)

φ_s＝h(α,β,γ)

则由以上关系式可以得到传感器测量值d的表达式：

d＝R-r＝R-f(θ_s,φ_s)＝R-f(g(α,β,γ),h(α,β,γ))

由此便建立了传感器测量值d与运动姿态(α,β,γ)之间的函数映射关系。

此处(θ_s,φ_s)与之前的(θ,φ)是等价相等的关系，可以理解为激光距离传感器发出的激光照射在球壳表面，激光照射点(在球壳上)和激光传感器在同一直线上，因此二者在球坐标下的(θ,φ)是相同的，即此处(θ_s,φ_s)和前面的(θ,φ)是等价相等的关系。由上可知，一个传感器可以给出一个方程，而一个方程中含有三个未知数α,β,γ，所以从数学角度，至少需要三个方程，即至少需要三个摆放在不同位置的传感器，才能唯一解出α,β,γ。

优选地，本实施例中的具体解法如下：从运动起始位置开始，将上一刻(α,β,γ)的值作为当前时刻的迭代初值，应用梯度下降法，可以解出当前时刻(α,β,γ)的精确数值解。

因此，利用本发明的方法，至少需要三个传感器就能够实现球关节姿态测量。如果设置多个传感器，则可以求出多组解并求均值。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。