[0035]计算四轴飞行器的三维坐标值，如图1所示，图中KZQ为定位控制器，X、Y、Z为三维坐标轴向示意，M为飞行器四个轴位置处均有安装的超声波接收器，为获得超声波测距最佳质量的数据，我们选择离定位控制器最近的三个超声波接收器，该三个接收器与定位控制器的连接距离图示为Xl、Χ2、Χ3;同时实施例的四轴飞行器的两个对角线长度相等，图示LSP为该连接线的长度;我们先假定四轴飞行器是水平的，且方位角为0，图示中四轴飞行器的中心位置相对定位控制器的三维坐标值为SX、SY、SZ，该三维坐标值的计算方法可以用如下办法求得:
SX=((X1*X1+X2*X2)*0.5-X3*X3)/L
SY=(X1*X1-X2*X2)/(L*2)
SZ=SQRT((X1*X1+X2*X2)*0.5_L*L*0.25_SX*SX_SY*SY)
通常情况下，四轴在空中都不是水平的，方位角也会经常变化，所以我们的四轴飞行器安装有惯性测量单元，用来检测四轴的姿态角，我们对上述的三维坐标值依据姿态角进行三次旋转，即可移除姿态角对四轴定位的误差影响，旋转一次影响两个坐标值，如图2所示，设姿态夹角为a，则旋转后的新坐标值可用如下公式求得:
SXl=SX*cos(a)_SZ*sin(a)
SZl=SX*sin(a)+SZ*cos(a)
其余两次旋转，参考本办法即可求出，本文不再敖述。
[0036]要说明的是，在计算姿态方位角时，可以没有磁传感器参与，但此时计算结果中的北方位角是假定的，与实际的地磁北将有差值，并且会有轻微漂移。
[0037]至此，我们的四轴飞行器已经完成基本定位计算，但由于超声波测距结果只是标量，无法识别出四轴控制器位于四轴平面的上部还是下部，为了全方位地应用，如图3所示，在四轴的下面安装有一个超声波接收单元M5，该接收单元与四轴控制器的距离为X5，我们由超声波距离值Xl、X2计算出四轴中心点到四轴控制器的距离值X0，比较X0、X5大小，从而可判断出四轴控制器位于四轴飞行器的下部。
[0038]在本发明的实施方案中，四轴飞行器上还安装有相机，用wifi遥控控制拍照、摄像及传输影数据，使用WIFI传输四轴控制命令，控制四轴飞行器的各种动作。
[0039]由于四轴飞行过程中，需消耗大量的电力，导致市面上的四轴绝大多数飞行不超过半小时，不能很好地满足人们旅行拍照的需求，所以本次实施方案中，定位控制器上配有大容量电池，预留接口通过五米左右的软性电源连接线与四轴飞行器相连，传输电力至四轴飞行器，使四轴飞行器飞行留空拍照时间超过半小时；当然也可以断开连接线，使用四轴上的电源供电，完成半小时内的留空拍照任务。
[0040]通过以上实施例，本领域的技术人员可以制作出一个定位精确的空中悬浮近身自拍旅行相机，但并非对本发明技术的限定，权利保护仍由本发明的权利要求书所涵盖。
【主权项】
1.一种基于惯性姿态与超声波测距的三维定位装置，其特征在于包括定位参照模块与待定位目标模块， 所述定位参照模块包括: 第一无线单元，能够与待定位目标模块进行无线通讯，收发管理控制指令与状态信息； 超声波发射单元，根据第一无线单元的指令，发送超声波信号； 所述待定位目标模块包括:惯性单元、第二无线单元、三个超声波接收单元及微处理器单元； 惯性单元包括陀螺仪传感器、加速度传感器，能够用来测算待定位目标模块的姿态信息，包括横滚角、俯仰角、方位角； 第二无线单元能够与定位参照模块进行无线通讯； 超声波接收单元能够用来接收定位参照模块发出的超声波信号； 微处理器单元管理控制其它单元与模块，具有以下功能: (1)利用无线收发信号同步，计算超声波发与收的间隔时间，计算定位参照模块至待定位目标模块上每个超声波接收单元的距离； (2)利用定位参照模块至待定位目标模块上三个超声波接收单元的距离值，以及三个超声波接收单元之间的固定距离值，还有待定位目标模块的姿态值，计算获得待定位目标模块相对于原点为定位参照模块的大地三维坐标值。2.根据权利要求1所述的装置，其特征在于:所述惯性测量单元还包括三轴磁传感器，用来校正权利要求1中所述姿态值中的方位角至地磁方位角。3.根据权利要求1或2所述的装置，其特征在于:所述三个超声波接收单元被分配在待定位目标模块上尽可能彼此距离远的位置。4.根据权利要求1至3中任一权利要求所述的装置，其特征在于:所述待定位目标模块上面还安装第四个超声波接收单元，且不安装在前述三个接收单元所形成的平面上，该单元与定位参照模块的距离与前述三个距离值进行计算比较，用来确定定位参照模块是位于前述三个接收单元所形成的平面的上部还是下部。5.根据权利要求1至4中任一权利要求所述的装置，其特征在于:所述待定位目标模块上面还另外安装有至少一个超声波接收单元，该超声波单元测量值用来替换或校正其它超声波接收单元的测量值。6.根据权利要求1至5中任一权利要求所述的装置，其特征在于:所述定位参照模块上面安装有电池;所述定位参照模块与待定位目标模块之间有软性电源线连接，定位参照模块通过该电源线向待定位目标模块输送电力;所述待定位目标模块上还安装有旋翼飞行器设备。7.根据权利要求1至6中任一权利要求所述装置，其特征在于:所述待定位目标模块安装有旋翼飞行器设备，该设备包括电调、电机、螺旋桨，且不少于四套;所述待定位目标模块上安装有相机，该相机可以被遥控指令所操作。8.基于权利要求1至7中任一所述的一种基于惯性姿态与超声波测距的三维定位装置的定位方法，其特征在于包括如下步骤: 获取定位参照模块至待定位目标模块上三个超声波接收单元的距离值； 获取待定位目标模块上惯性单元的姿态值，该姿态值包括横滚角、俯仰角、方位角； 利用定位参照模块至待定位目标模块上三个超声波接收单元的距离值，以及三个超声波接收单元之间的固定距离值，还有待定位目标模块的姿态值，计算获得待定位目标模块相对于原点为定位参照模块的大地三维坐标值。9.如权利要求8所述方法，其特征是:获取待定位目标模块上三轴磁传感器测量值，校正所述姿态值中的方位角至地磁方位角。10.如权利要求8或9所述的方法，其特征在于:获取第四个超声波接收单元与定位参照模块的距离值，根据该距离值与前三个超声波距离值比较，来判断定位参照模块是位于前三个超声波接收单元所连成平面的上部或下部。11.如权利要求8至10中的任一所述方法，其特征在于:至少再获取一个超声波接收单元与定位参照模块的距离值，该超声波距离值，用来替换或校正其它超声波距离值。
【专利摘要】本发明提供一种基于惯性姿态与超声波测距的定位方法及装置，在本发明中，地面定位参照模块包括超声波发射单元及无线同步单元，待定位目标模块包括惯性单元、超声波接收单元及无线同步单元，利用地面参照模块到待定位目标模块上的不少于三个超声波距离值，结合待定位目标模块的姿态数据，计算获得待定位目标模块的三维坐标值，该定位发明应用于四轴飞行器中，除四轴外，只需携带一个地面装置，即可实现对四轴进行全天候、全地域的精确飞行控制，特别适合于近身空中相机拍照应用。
【IPC分类】G01C21/18, G01S5/22
【公开号】CN105572638
【申请号】CN201610081788
【发明人】向杰
【申请人】向杰
【公开日】2016年5月11日
【申请日】2016年2月6日