CN101947150B - 实现多自由度假肢对随机目标进行空间定位的方法 - Google Patents

Abstract

本发明公开了一种实现多自由度上假肢对随机目标进行空间定位的方法，它分别在多自由度上假肢上安装一个三维姿态传感器；在佩戴者的头部或帽子上安装另外一个三维姿态传感器；在佩戴者的头部或帽子上再安装一个激光测距传感器；为佩戴者配备一个假肢控制器，每当出现需要操作的随机目标时，通过佩戴者自主调整头部的姿态，将激光测距传感器发出的激光束对准目标，并由控制器发出指令，实时测出目标与激光测距传感器间的距离值，同时由控制器计算、转换为目标在假肢基础坐标系中实时的操作空间坐标值。本发明为工作在非结构化环境中的肩离断形残疾人多自由度上假肢对随机目标进行实时操作空间定位的方法。

Description

实现多自由度假肢对随机目标进行空间定位的方法

技术领域

本发明涉及肩离断型残疾人假肢，尤其涉及上假肢(指上肢假肢)对其随机目标进行操作空间定位的方法。该方法也可以用在其它具有类似工作情形的机器人对随机目标的操作空间定位上。

背景技术

对用在非结构化环境中的“肩离断”型残疾人上假肢，尤其是可实现代偿功能的多自由度上假肢来说，如何实现其对随机目标的操作空间位置实时定位，以能按照要求完成操作任务，在以往的多自由度上假肢研究中，尚无对该技术方法的研究。通过本发明以获得求解假肢各关节运动量的必要条件，实现多自由度上假肢按任务要求完成操作任务，是本领域研究人员亟需解决的问题。

发明内容

本发明的目的，是为多自由度上假肢对随机目标能够实时操作空间定位，提出一种实现方法。

为达上述目的，本发明总的解决方案是，通过采用分别安装在佩戴者头部和假肢上的两个三维姿态传感器，对佩戴者头部的空间姿态相对于上假肢基础姿态的变化情况进行检测，并采用激光测距传感器对随机目标进行距离检测，再通过在不同坐标系中连续的坐标变换推演，计算出随机目标相对于多自由度上假肢的操作空间定位值。

具体方案如下：

1、在多自由度上假肢上安装一个三维姿态传感器，该传感器用作假肢基础的标准姿态校准；

2、在佩戴者的头部或帽子上安装另外一个三维姿态传感器，该三维姿态传感器用来检测佩戴者头部左右转动或上下俯仰时，其头部的空间姿态相对假肢基础标准姿态所产生的变化情况。

3、调整上述两个三维姿态传感器的初始状态，初始状态定为：当佩戴者头部端正平视正前方时，两个三维姿态传感器的三维姿态角相同。

4、在佩戴者的头部或帽子上再安装一个激光测距传感器，激光测距传感器以某种位姿固定在佩带者头部，通过调整佩戴者头部左右转动和上下俯仰，使该激光测距传感器发出的可见激光束指向需要操作的随机目标。

5、为佩戴者配备一个假肢控制器，假肢控制器存储有计算随机目标操作空间坐标值P的计算程序，P的计算式为：

$P=K_N^S\·K_H^N\·K_E^H\·M---\left(1\right)$

其中，S代表肩部坐标系(即假肢的基础坐标系)，N代表颈根部正中坐标系，H代表头部正中坐标系，E代表激光测距传感器的坐标系，M代表激光传感器测得的操作目标坐标值，可由激光传感器实时测得，K代表坐标变换矩阵。

它们的关系是：

在坐标系E中描述操作目标的坐标值M，M值由激光测距传感器获得；

在坐标系H中描述坐标系E的位姿，E相对H的位姿变换取决于激光测距传感器在头部的安装方位，可标定，属于定值，即：公式中的

标定后属于定值。

在坐标系N中描述坐标系H的位姿，当佩戴者头部端正平视正前方时的坐标系H相对坐标系N的初始方位可标定，属于定值。它主要取决于佩戴者颈部与头的长度因素，当佩戴者头部左右转动时，相当于坐标系H绕坐标系N的z_n轴转动，坐标系H相对坐标系N的姿态就发生变化，通过比较两个三维姿态传感器该方向之间的姿态角偏差值，可以推导求得坐标系H相对坐标系N的姿态变化幅度，即：公式中的通过比较两个三维姿态传感器之间的相关姿态角偏差值，经实时换算、推导求出。

在坐标系S中描述坐标系N的位姿，当佩戴者头部端正平视正前方时的坐标系N相对坐标系S的初始方位可标定，属于定值，它主要取决于佩戴者肩部的宽度及其与颈部中央的前后位置参数。当佩戴者头部上下俯仰时，相当于坐标系N绕坐标系S的y_s轴转动，坐标系N相对坐标系S的姿态就发生变化。通过比较两个三维姿态传感器该方向之间的姿态角偏差值，可以推导求得坐标系N相对坐标系S的姿态变化幅度，即：公式中的

通过比较两个三维姿态传感器之间的相关姿态角偏差值，经实时换算、推导求出。

6、每当出现需要操作的随机目标时，通过佩戴者自主调整头部的姿态，将激光测距传感器发出的激光束对准目标，并由控制器发出指令，实时测出目标与激光测距传感器间的距离值M，同时由控制器计算比较前述两个三维姿态传感器之间的相关姿态角偏差值，求得值与

值，控制器再将实时获得的M值、

与

值，连同已经标定的

代入公式(1)相乘，就可转换为操作目标在假肢基础坐标系中实时的空间坐标值P。

本发明的优点在于：

发明了一种为工作在非结构化环境中的“肩离断”形残疾人多自由度上假肢对随机目标进行实时空间定位的方法。定位后可为多自由度上假肢实时指明目标所在的操作空间位置，以使其完成指定的操作任务。

附图说明

参照附图1、图2、图3对本发明进一步说明。

图1是假肢佩戴者与对随机目标实时进行空间定位系统的一种初始状态的安装关系图。其中图1(a)是主视示意图，图1(b)是俯视示意图。

图2是佩戴者头部前方有一物体时，佩戴者头部端平，正好将激光传感器发出的激光束对准目标进行测距的示意图。图中仅示出头部或帽子上安装的三维姿态传感器与假肢基础上安装的三维姿态传感器的姿态角无偏差情况。

图3是佩戴者头部右下前方有一物体时，佩戴者调整头部姿态将激光测距传感器发出的激光束对准目标进行测距的示意图。图中仅示出头部或帽子上安装的三维姿态传感器与假肢基础上安装的三维姿态传感器的姿态角出现偏差，经过换算可以推导出头部姿态相对于假肢基础的姿态变化情况。

图例说明：1-多自由度假肢，2-三维姿态传感器，3-三维姿态传感器，4-激光测距传感器，E-激光测距传感器的坐标系，H-头部正中的坐标系，N-颈根正中的坐标系，S-肩部坐标系(即假肢的基础坐标系)。

具体实施方式

如图1-3所示，本发明技术方案包括在多自由度上假肢1的基础上安装一个三维姿态传感器2，在佩戴者的头部帽子上安装一个三维姿态传感器3，在佩戴者的头部帽子上安装一个激光测距传感器4，为佩戴者配备一个假肢控制器，假肢控制器存储有可以实时计算操作随机目标空间坐标值P的计算程序。P值的计算式及其代表符号之间的关系如技术方案中的公式(1)所述。计算式中建立的各个坐标系的初始姿态如图1中所示。下面以图1、图2、图3说明两个对随机目标实时进行空间定位的例子。

1、首先如图1所示设某坐标系绕其它某坐标系x轴的旋转角为θ，绕y轴的旋转角为β，绕z轴的旋转角为φ，则，N坐标系绕S坐标系x_s、y_s、z_s轴的旋转角就分别记为θ_s、β_s、φ_s，H坐标系绕N坐标系x_n、y_n、z_n轴的旋转角就分别记为θ_n、β_n、φ_n，E坐标系绕H坐标系x_h、y_h、z_h轴的旋转角就分别记为θ_h、β_h、φ_h；设N坐标系原点离开S坐标系原点三方向上的距离分别记为x_s、y_s、z_s，H坐标系原点离开N坐标系原点三方向上的距离分别记为x_n、y_n、z_n，E坐标系原点离开H坐标系原点三方向上的距离分别记为x_h、y_h、z_h。

2、通过对佩戴者本身某些参数进行定义与标定，假若可获得如下参数：

N坐标系相对S坐标系初始时有以下参数θ_s＝0°，β_s＝0°，φ_s＝0°，x_s＝20mm，y_s＝200mm，z_s＝50mm，知道N坐标系只可以绕S坐标系的y_s轴转动，用齐次方程表示，则初始时有：

$K_N^S=\left[\begin{array}{cccc}\cos \left({\mathrm{\β}}_s\right)& 0& \sin \left({\mathrm{\β}}_s\right)& x_s\\ 0& 1& 0& y_s\\ -\sin \left({\mathrm{\β}}_s\right)& 0& \cos \left({\mathrm{\β}}_s\right)& z_s\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 20\\ 0& 1& 0& 200\\ 0& 0& 1& 50\\ 0& 0& 0& 1\end{array}\right]$

H坐标系相对N坐标系初始时有以下参数θ_n＝0°，β_n＝0°，φ_n＝0°，x_n＝0mm，y_n＝0mm，z_n＝200mm，知道H坐标系只可以绕N坐标系的z_n轴转动，用齐次方程表示，则初始时有：

$K_H^N=\left[\begin{array}{cccc}\cos \left({\mathrm{\φ}}_n\right)& -\sin \left({\mathrm{\φ}}_n\right)& 0& x_n\\ \sin \left({\mathrm{\φ}}_n\right)& \cos \left({\mathrm{\φ}}_n\right)& 0& y_n\\ 0& 0& 1& z_n\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 200\\ 0& 0& 0& 1\end{array}\right]$

E坐标系相对H坐标系初始时有以下参数θ_h＝0°，β_h＝0°，φ_h＝-15°，x_h＝0mm，y_h＝130mm，z_h＝0mm，知道E坐标系只可以绕H坐标系的z_h轴有一个固定的转角，用齐次方程表示，则

总是有：

3、如图2所示当佩戴者活动头部，将激光测距传感器的激光束对准目标时，由激光测距传感器实时测得以下用齐次方程表示的目标距离值(单位：mm)：

$M=\left[\begin{array}{c}{m}_x\\ m_y\\ m_z\\ 1\end{array}\right]=\left[\begin{array}{c}550\\ 0\\ 0\\ 1\end{array}\right]$

比较两个姿态传感器偏差可得β_s＝0°，φ_n＝0°，则：

将上述参数实时输入控制器中编写的公式(1)的程序中，可以计算获得目标相对假肢基础坐标系的空间位置是：

$P=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\\ 1\end{array}\right]=\left[\begin{array}{c}551.2592\\ 187.6495\\ 250.0000\\ 1\end{array}\right]$

即：相对于假肢基础坐标系，目标在其原点x方向的551.2592mm处，y方向的187.6495mm处，z方向的250.0000mm处。

4、另一实例如图3所示：

当佩戴者活动头部，将激光测距传感器的激光束对准目标时，由激光测距传感器实时测得以下用齐次方程表示的目标距离值(单位：mm)：

$M=\left[\begin{array}{c}{m}_x\\ m_y\\ m_z\\ 1\end{array}\right]=\left[\begin{array}{c}600\\ 0\\ 0\\ 1\end{array}\right]$

比较两个姿态传感器偏差可得β_s＝35°，φ_n＝-10°，则：

将上述参数实时输入控制器中编写的公式(1)的程序中，可以计算获得目标相对假肢基础坐标系的空间位置是：

$P=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\\ 1\end{array}\right]=\left[\begin{array}{c}598.6494\\ 74.4541\\ -111.0197\\ 1\end{array}\right]$

即：相对于假肢基础坐标系，目标在其原点x方向的598.6494mm处，y方向的74.4541mm处，z方向的-111.0197mm处。

本发明说明书中未详细描述的内容属于本领域专业技术人员公知的现有技术或理论。

Claims (1)

1.一种实现多自由度上假肢对随机目标进行空间定位的方法，其特征在于，步骤如下：

第一、在多自由度上假肢上安装一个第一三维姿态传感器，第一三维姿态传感器用作假肢基础的标准姿态校准；

第二、在佩戴者的头部安装一个第二三维姿态传感器，第二三维姿态传感器用来检测佩戴者头部左右转动或上下俯仰时，其头部的空间姿态相对假肢基础标准姿态所产生的变化情况；

第三、调整上述第一、第二两个三维姿态传感器的初始状态，初始状态定为：当佩戴者头部端正平视正前方时，两个三维姿态传感器的三维姿态角相同；

第四、在佩戴者的头部再安装一个激光测距传感器，激光测距传感器以某种位姿固定在佩戴者头部，通过调整佩戴者头部左右转动和上下俯仰，使该激光测距传感器发出的可见激光束指向需要操作的随机目标；

第五、为佩戴者配备一个假肢控制器，假肢控制器存储有计算操作随机目标空间坐标值P的计算程序，P的计算式为：

$P=K_N^S\·K_H^N\·K_E^H\·M---\left(1\right)$

其中，S代表肩部坐标系，N代表颈根部正中坐标系，H代表头部正中坐标系，E代表激光测距传感器的坐标系，M代表激光测距传感器测得的操作目标坐标值，可由激光测距传感器实时测得，K代表坐标变换矩阵；

它们的关系是：

在坐标系E中描述操作目标的坐标值M，M值由激光测距传感器获得；

在坐标系H中描述坐标系E的位姿，E相对H的位姿变换取决于激光测距传感器在头部的安装方位，可标定，属于定值，即：公式中的

标定后属于定值；

在坐标系N中描述坐标系H的位姿，当佩戴者头部端正平视正前方时的坐标系H相对坐标系N的初始方位可标定，属于定值；它主要取决于佩戴者颈部与头的长度因素，当佩戴者头部左右转动时，相当于坐标系H绕坐标系N的z_n轴转动，坐标系H相对坐标系N的姿态就发生变化，通过比较两个三维姿态传感器之间的姿态角偏差值，可以推导求得坐标系H相对坐标系N的姿态变化幅度，即：公式中的

通过比较两个三维姿态传感器之间的相关姿态角偏差值，经实时换算、推导求出；

在坐标系S中描述坐标系N的位姿，当佩戴者头部端正平视正前方时的坐标系N相对坐标系S的初始方位可标定，属于定值，它主要取决于佩戴者肩部的宽度及其与颈部中央的前后位置参数；当佩戴者头部上下俯仰时，相当于坐标系N绕坐标系S的y_s轴转动，坐标系N相对坐标系S的姿态就发生变化；通过比较两个三维姿态传感器之间的姿态角偏差值，可以推导求得坐标系N相对坐标系S的姿态变化幅度，即：公式中的

通过比较两个三维姿态传感器之间的相关姿态角偏差值，经实时换算、推导求出；

第六、每当出现需要操作的随机目标时，通过佩戴者自主调整头部的姿态，将激光测距传感器发出的可见激光束对准目标，并由控制器发出指令，实时测出目标与激光测距传感器间的距离值M，同时由控制器计算比较前述两个三维姿态传感器之间的相关姿态角偏差值，求得

值与

值，控制器再将实时获得的M值、

与

值，连同已经标定的

代入公式(1)相乘，就可转换为目标在假肢基础坐标系中实时的操作空间坐标值P。

Images