[发明专利]一种新型旋翼类无人机自动定位操控方法

摘要

本发明涉及无人机技术领域，具体公开了一种新型旋翼类无人机自动定位操控方法，具体为用带有方向感应功能和位置定位功能的电子设备，多旋翼类无人机按照电子设备的方向和位置来控制自身的方向和位置，从而实现旋翼类无人机的方向和位置随动。主要是构建了动态的相对坐标系，测量信息从测量坐标系转换至相对坐标系下，使用了定位电子设备的姿态和位置信息，其采用了AGPS、陀螺加磁罗盘的定位定姿方法，降低了相对测量受成本和精度影响的问题，从而保证了较高精度的定位定姿手段。利用相对信息进行多旋翼无人机的相对位置和姿态控制，并最终将其转换至测量坐标系下进行绝对控制。

主权项

一种新型旋翼类无人机自动定位操控方法，其特征在于：包括带有方向感应功能和位置定位功能的电子设备，所述电子设备通过无线方式和旋翼类无人机建立联接，具体步骤如下：(1)电子设备给出在测量坐标系下的经度、纬度、高度以及航向角，定义为λ,B,h,γ，在测量坐标系下，该值为地理坐标系的位置以及航向角；(2)利用无人旋翼机所具有的机上测量装置，可以获得在测量的地理坐标系下的经纬度、高度和航向角，定位为λT,BT,hT,γT；(3)构建相对坐标系，基于绝对测量位置获得相对位置关系：假设在有限范围内，忽略地球的曲率变化，定义当地的基准坐标系，坐标原点为λ,B,h，x轴指向电子设备的航向角γ方向，y轴定位为垂直地面，向上为正，则z轴构成右手坐标系；根据电子设备所具有的地理位置和航向，以及多旋翼无人机所具有的地理位置和航向，进行坐标变换，计算得到相对坐标系下多旋翼无人机的相对位置和航向角；将地理坐标系位置进行地心坐标系的变化，根据GPS接收机的信号，其基于美国WGS84坐标系，进行如下坐标变化，得到其经纬高如下：首先定义基本地球坐标参数地球长半径：a＝6378137±2(m)；地球引力和地球质量的乘积：GM＝3986005×108m3/s2±0.6×108m3/s2；正常化二阶带谐系数：C20＝‑484.16685×10‑6±1.3×10‑9；地球重力场二阶带球谐系数：J2＝108263×10‑8；地球自转角速度：ω＝7292115×10‑11rad/s±0.150×10‑11rad/s；扁率f＝0.003352810664以此为基础进行GPS接收数据的地心坐标系转换；Lr＝L·π/180N=a/1-e2sin(B)2]]>x＝(N+H)cos(B)cos(Lr)       (1)y＝(N+H)cos(B)sin(Lr)z＝(N(1‑e2)+H)sin(B)式中，L,B,H为接收机直接接收得到的信号。根据所得到经纬高，将其转换为地心坐标系下的坐标x,y,z；(4)根据电子设备基于接收机得到的坐标系xh,yh,zh，以及磁罗盘给出的方位角γ，将电子设备通过无线方式和旋翼类无人机联通，电子设备可以将相应的控制信号传送至旋翼类无人机上；(5)旋翼类无人机上的控制器接收到电子设备感应到的方向信息和其位置信息后，在此基础上生成自身的方向控制给定和位置控制给定：旋翼类无人机根据自身定位设备所得到的地心坐标系坐标xa,ya,za，进行相对位置计算；定义地面坐标系如下：以基准电子设备所在位置作为坐标原点，x轴指向以方位角为准，所在位置与地心连线设为y轴，向上为正，Z轴构成右手坐标系；假设飞机距离电子设备在千米范围内，可忽略地球曲率，认为地面坐标系x轴沿地面延伸；基于飞行的地心坐标，则可以得到飞行相对于电子设备的相位位置关系如下：G=-sinγsinL-cosγsinBcosLsinγcosL-cosγsinBsinLcosγcosBcosBcosLcosBsinLsinB-cosγsinL+sinγsinBcosLcosγcosL+sinγsinBsinL-sinγcosB---(2)]]>则有地面坐标系下可以得到飞行相对于电子设备所在地表点的相关坐标如下：xyz=Gxayaza---(3)]]>根据电子设备本身的高度信息，修正计算得到基于电子设备的相对位置为xryrzr=xy-Hz---(4)]]>旋翼类无人机上的控制器收到电子设备感应到的方向信息和其位置信息后，生成位置控制给定，也可以叠加一个可变或不变量后再将其作为旋翼类无人机的方向控制给定；(6)以电子设备的位置为圆心，以旋翼类无人机同电子设备之间的相对距离为半径作圆；以电子设备的位置为直线上的一点，以电子设备的方向为斜率，作一条射线；所作的射线和圆必有一个交点，将该交点作为位置控制的给定；根据式(4)，可以得到飞行器相对于电子设备的同心圆半径为：R=xr2+yr2+zr2---(5)]]>则有相对位置控制指令为：xrc＝R,yrc＝yr,zrc＝0      (6)同时飞行器的航向角指令可计算为：r+π,γ<πγ-π,γ≥π---(7)]]>(7)在控制指令生成后，基于所需要的控制指令，进行位置与航向的控制实现。对于一般的旋翼类无人机的位置回路，其主要的控制机构为电机转速，方位的控制通过姿态控制实现，则可以给出以下的实现步骤；位置控制通道可分为水平控制通道和高度控制通道，由于高度控制与角度不存在耦合关系，因此可以直接进行，得到如下的控制量；y··=kp3(yrc-yr)+kd3d(yrc-yr)dt---(8)]]>根据期望的高度角速度，反向阶段升力的输入信号，根据以下的旋翼动力学方程获取相关值；U1=1cosφcosθ(kp3(yrc-yr)+kd3(y·rc-y·r)+mg)---(9)]]>式中，分别取系数为kp3＝2,kd3＝0.8；得到的总升力一般根据旋翼的个数进行分解，根据四旋翼、六旋翼和八旋翼进行相应的转速分解。此外还需要考虑水平位置控制通过的俯仰和滚转角进行反解算；水平位置控制采用PID控制器，以期望目标相对位置(xrc,zrc)和实际相对位置(xr,zr)的差作为输入信号，形成如下的控制形式：x··=kp1(xrc-xr)+ki1∫(xrc-xr)dt+kd1(x·rc-x·r)]]>                         (10)z··=kp1(zrc-zr)+ki1∫(zrc-zr)dt+kd1(z·rc-z·r)]]>式中，系数分别设定为kp1＝2.3,ki1＝0.3,kd1＝0.8；kp2＝2.3,ki2＝0.3,kd2＝0.8；将水平位置的加速度值作为反结算模块的输入信息，基于多旋翼飞行器的运动学和动力学方程，做以下的推导。在多旋翼悬停与慢速飞行过程中，可假设角速度较小，则运动学方程中交叉项约为零，可将质心的动力学和运动学模型写为x··=(sinθcosφcosψ+sinφsinψ)U1m]]>                  (11)z··=(sinθcosφsinψ-sinφcosψ)U1m]]>对式(11)中的方程1两边乘以sinψ，方程2两边乘以cosψ，方程1减去方程2可得mx··sinψ-mz··cosψ=U1sinφ]]>φd=arcsin[mU1(x··sinψ-y··cosψ)]---(12)]]>而对式(11)中的第一个方程变形，则可以得到sinθ=[mx··U1-sinφsinψ]/cosφcosψ]]>                (13)则此时可以获得姿态内回路的俯仰角、滚转角以及偏航角的指令(φc,θc,ψc)，同时还可以获得基本的控制量U1；(8)根据姿态指令进行机动过程中的姿态控制，并在位置到达后将姿态指令全部置零，除了航向角保持指令航向角；则可以得到多轴电机融合后的输入控制量为：U2=kpφeφ+kdφdeφdt+kiφ∫eφ]]>U3=kpθeθ+kdθdeθdt+kiθ∫eθ---(14)]]>U4=kpψeψ+kdψdeψdt+kiψ∫eψ]]>综合以上则可以得到多旋翼无人机的典型四路输入信号U1,U2,U3,U4。