自动航线飞行分析.pdf-资料库

j1124427436-9973341-自动航线飞行分析.pdf-第1页.png

第1页 / 共23页

j1124427436-9973341-自动航线飞行分析.pdf-第2页.png

第2页 / 共23页

j1124427436-9973341-自动航线飞行分析.pdf-第3页.png

第3页 / 共23页

j1124427436-9973341-自动航线飞行分析.pdf-第4页.png

第4页 / 共23页

j1124427436-9973341-自动航线飞行分析.pdf-第5页.png

第5页 / 共23页

j1124427436-9973341-自动航线飞行分析.pdf-第6页.png

第6页 / 共23页

j1124427436-9973341-自动航线飞行分析.pdf-第7页.png

第7页 / 共23页

j1124427436-9973341-自动航线飞行分析.pdf-第8页.png

第8页 / 共23页

雷神空天-无人机植保 http://www.sclskt.com/ 自动航线实现分析 Author:jingwenyi Date:2017.09.11 自动航线实现分析.............................................................................................................................1 一、导航层分析.......................................................................................................................2 1、初始化实现.................................................................................................................2 2、轨道飞行控制.............................................................................................................4 二、位置控制层分析...............................................................................................................8 1、xy 平面的位置控制...................................................................................................... 8 2、垂直平面的位置分析...............................................................................................12 三、姿态控制层分析.............................................................................................................14 1、四元素相关知识.......................................................................................................14 2、垂直方向的姿态控制...............................................................................................18 3、 xy 平面姿态控制...................................................................................................... 20 4、计算电机的输出.......................................................................................................22 1

雷神空天-无人机植保 http://www.sclskt.com/ 我个人的理解飞控实现自动航线分了三个层次：导航层、位置控制层、姿态控制层。一、导航层分析飞控的实现代码在 ac_wpnav.cpp 中 1、初始化实现初始化函数 set_wp_origin_and_destination，初始化的时候需要输入的参数有 origin 点的向量坐标，目的地向量坐标，是否防地标志。初始化过程首先计算出目的地到 origin 点的轨迹长度_track_length； Vector3f pos_delta = _destination - _origin; _track_length = pos_delta.length(); // get track length 计算出同 origin 到 destination 的单位向量_pos_delta_unit； if (is_zero(_track_length)) { // avoid possible divide by zero _pos_delta_unit.x = 0; _pos_delta_unit.y = 0; _pos_delta_unit.z = 0; }else{ _pos_delta_unit = pos_delta/_track_length; } 计算控制器的控制距离 // calculate leash lengths calculate_wp_leash_length(); void AC_WPNav::calculate_wp_leash_length() { //求出在水平方向上单位向量的长度 float pos_delta_unit_xy = norm(_pos_delta_unit.x, _pos_delta_unit.y); //求出垂直方向上单位向量的长度 float pos_delta_unit_z = fabsf(_pos_delta_unit.z); //通过垂直方向距离的正负求出获得对应的加速度和控制距离 float speed_z; float leash_z; if (_pos_delta_unit.z >= 0.0f) { speed_z = _wp_speed_up_cms; leash_z = _pos_control.get_leash_up_z(); }else{ speed_z = _wp_speed_down_cms; leash_z = _pos_control.get_leash_down_z(); } 2

雷神空天-无人机植保 http://www.sclskt.com/ //分 4 种情况计算最大加速度、最大速度、控制距离 if(is_zero(pos_delta_unit_z) && is_zero(pos_delta_unit_xy)){ //水平的和垂直的单位向量都为 0 _track_accel = 0; _track_speed = 0; _track_leash_length = WPNAV_LEASH_LENGTH_MIN; }else if(is_zero(_pos_delta_unit.z)){ // 求出轨道水平方向可以获得的最大加速度、速度、控制距离 _track_accel = _wp_accel_cms/pos_delta_unit_xy; _track_speed = _wp_speed_cms/pos_delta_unit_xy; _track_leash_length = _pos_control.get_leash_xy()/pos_delta_unit_xy; }else if(is_zero(pos_delta_unit_xy)){ //求出轨道垂直可以获得的最大加速度、速度、控制距离 _track_accel = _wp_accel_z_cms/pos_delta_unit_z; _track_speed = speed_z/pos_delta_unit_z; _track_leash_length = leash_z/pos_delta_unit_z; }else{ //求出水平方向和垂直方向都可以接受的加速度、速度和控制距离（所以这里取了水平和垂直方向较小的一方） _track_accel=MIN(_wp_accel_z_cms/pos_delta_unit_z,_wp_accel_cms/pos_delta_unit _xy); _track_speed = MIN(speed_z/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy); _track_leash_length=MIN(leash_z/pos_delta_unit_z,_pos_control.get_leash_xy()/pos_ delta_unit_xy); } //通过当前的速度和加速度，求得减速需要的距离 calc_slow_down_distance(_track_speed, _track_accel); } 初始化位置控制器，把当前点设置成控制器要到达的第一个中间点 _pos_control.set_pos_target(origin + Vector3f(0,0,origin_terr_offset)); 初始化期望轨道距离 _track_desired = 0; 初始化到达目的地的标志 _flags.reached_destination = false; 初始化沿轨道的速度 //获得当前速度 const Vector3f &curr_vel = _inav.get_velocity(); //把当前速度投影到轨道上 float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + 3

雷神空天-无人机植保 http://www.sclskt.com/ curr_vel.z * _pos_delta_unit.z; //把轨道速度限制在范围内 _limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms); 2、轨道飞行控制按着轨道飞行的函数是 advance_wp_target_along_track，思路是以当前飞机的位置、速度加速度，求出下一个时间片期望的目标点。获取当前飞机的位置 Vector3f curr_pos = _inav.get_position(); 求出当前位置相对于 origin 的相对距离向量 Vector3f curr_delta = (curr_pos - Vector3f(0,0,terr_offset)) - _origin; 求出当前位置在轨道上的投影距离 track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z; 求出轨道上对应的位置 Vector3f track_covered_pos = _pos_delta_unit * track_covered; 求出当前位置和轨道上对应的位置差 track_error = curr_delta - track_covered_pos; 求出水平差距离和高度差距离 float track_error_xy = norm(track_error.x, track_error.y); float track_error_z = fabsf(track_error.z); 求出水平和垂直的控制距离 float leash_xy = _pos_control.get_leash_xy(); float leash_z; if (track_error.z >= 0) { leash_z = _pos_control.get_leash_up_z(); }else{ leash_z = _pos_control.get_leash_down_z(); } 求出水平方向和垂直方向到达当前中间目标位置需要的距离，取较小的一个 _track_lea sh_length leash_z  track_leas leash_z - h_slack track_erro r_z track_leash_slack=MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, 4

雷神空天-无人机植保 http://www.sclskt.com/ _track_leash_length*(leash_xy-track_error_xy)/leash_xy); 计算需要在轨道上需要前进的距离 if (track_leash_slack < 0) { track_desired_max = track_covered; }else{ } track_desired_max = track_covered + track_leash_slack; 判断速度是否应该别限制，如果飞机飞的距离小于期望的距离，就应该增加速度而不是限制速度 if (_track_desired > track_desired_max) { reached_leash_limit = true; } 获得当前速度 const Vector3f &curr_vel = _inav.get_velocity(); //把速度分解到轨道上 float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z; 判断是否在向相反的方向飞行 if (speed_along_track < -linear_velocity) { //如果向相反的方向运动，把速度设置成 0 _limited_speed_xy_cms = 0; }else{ if(dt > 0 && !reached_leash_limit) { //速度不需要限制，继续加大速度 _limited_speed_xy_cms += 2.0f * _track_accel * dt; } //限制飞行速度范围 _limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms, 0.0f, _track_speed); //fast_waypoint 设置成 true 表示快速通过航点，飞行的现象是还没有到达航点就开始执行下一个航向了 if (!_flags.fast_waypoint) { //计算距离航点还有多远 float dist_to_dest = _track_length - _track_desired; //如果小于刹车距离，就把减速标志置为 true if (!_flags.slowing_down && dist_to_dest <= _slow_down_dist) { _flags.slowing_down = true; } // if target is slowing down, limit the speed 5

雷神空天-无人机植保 http://www.sclskt.com/ if (_flags.slowing_down) { //限制速度，开始减速 _limited_speed_xy_cms=MIN(_limited_speed_xy_cms, get_slow_down_speed(dist_to_dest, _track_accel)); } } //这里描述的一个情况是，如果当前轨道速度的大小小于在一个时间片的加速大小，就把期望速度限制到 speed_along_track-linear_velocity 和 speed_along_track+linear_velocity 范围内 if (fabsf(speed_along_track) < linear_velocity) { _limited_speed_xy_cms=constrain_float(_limited_speed_xy_cms, speed_along_track-linear_velocity,speed_along_track+linear_velocity); } } 计算前进的目标位置 if (!reached_leash_limit) { //轨道上期望的位置+上速度在单位时间移动的位置 _track_desired += _limited_speed_xy_cms * dt; // reduce speed if we reach end of leash if (_track_desired > track_desired_max) { _track_desired = track_desired_max; _limited_speed_xy_cms -= 2.0f * _track_accel * dt; if (_limited_speed_xy_cms < 0.0f) { _limited_speed_xy_cms = 0.0f; } } } 计算期望的位置 Vector3f final_target = _origin + _pos_delta_unit * _track_desired; 如果防地，计算期望的高度 final_target.z += terr_offset; 把期望的位置设置给位置控制层 _pos_control.set_pos_target(final_target); 判断是否到达航点 if( !_flags.reached_destination ) { if( _track_desired >= _track_length ) { if (_flags.fast_waypoint) { //到达航点 6

雷神空天-无人机植保 http://www.sclskt.com/ _flags.reached_destination = true; }else{ // regular waypoints also require the copter to be within the waypoint radius Vector3f dist_to_dest = (curr_pos - Vector3f(0,0,terr_offset)) - _destination; if( dist_to_dest.length() <= _wp_radius_cm ) { //到达航点 _flags.reached_destination = true; } } } } 7

雷神空天-无人机植保 http://www.sclskt.com/ 二、位置控制层分析 1、xy 平面的位置控制位置控制层的代码在 AC_poscontrol.cpp 中 update_xy_controller，该函数有 3 个参数 false); _pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_ONLY, XY_MODE_POS_ONLY 表示只通过位置修正，不设置前馈速度。 1.0f, 位置控制的思路有 3 种，分别是用位置修正、前馈速度修正、位置和前馈速度修正在自动执行航线时用的是位置修正，但在初始化时没有把前馈速度置 0，不知道为什么会这样。按照这样前馈速度不为 0，先通过前馈速度更新目标位置 desired_vel_to_pos(dt); 8

资料库

自动航线飞行分析.pdf

相关推荐

人工智能

热门标签

最新资料