资讯详情

PX4从放弃到精通（二十）：无人车(船)

文章目录

  • 前言
  • 一、主程序
  • 二、位置控制
  • 三、速度控制
  • 四、姿势控制
  • 五、L1导航
  • 六、调参

前言

PX4固件版本:1.11.3

一、主程序

程序位置 在这里插入图片描述 订阅所需的数据并设置订阅频率

void RoverPositionControl::run() { 
          _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));  _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));  _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));  _manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));  _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));  _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));   _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));  _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));   /* rate limit control mode updates to 5Hz */  orb_set_interval(_control_mode_sub, 200);   /* rate limit position updates to 50 Hz */  orb_set_interval(_global_pos_sub, 20);  orb_set_interval(_local_pos_sub, 20);

更新参数

parameters_update(true);

阻塞等待

/* wakeup source(s) */
	px4_pollfd_struct_t fds[5];

	/* Setup of loop */
	fds[0].fd = _global_pos_sub;
	fds[0].events = POLLIN;
	fds[1].fd = _manual_control_setpoint_sub;
	fds[1].events = POLLIN;
	fds[2].fd = _sensor_combined_sub;
	fds[2].events = POLLIN;
	fds[3].fd = _vehicle_attitude_sub; // Poll attitude
	fds[3].events = POLLIN;
	fds[4].fd = _local_pos_sub;  // Added local position as source of position
	fds[4].events = POLLIN;

大循环

while (!should_exit()) {

等待数据

/* wait for up to 500ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
	/* this is undesirable but not much we can do - might want to flag unhappy status */

没有数据则不执行下面的语句，重新循环

if (pret < 0) { 
        
	warn("poll error %d, %d", pret, errno);
	continue;
}

如果消息更新，则订阅消息 具体定义如下： 

/* check vehicle control mode for changes to publication state */
vehicle_control_mode_poll();
attitude_setpoint_poll();

订阅加速度

_vehicle_acceleration_sub.update();

跟新参数

/* update parameters from storage */
parameters_update();

手动模式标志

bool manual_mode = _control_mode.flag_control_manual_enabled;

如果全球位置和本地位置跟新，则运行控制器

/* only run controller if position changed */
if (fds[0].revents & POLLIN || fds[4].revents & POLLIN) { 
        
	perf_begin(_loop_perf);

订阅全球位置和本地位置

/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
position_setpoint_triplet_poll();

如果是offboard模式

//Convert Local setpoints to global setpoints
if (_control_mode.flag_control_offboard_enabled) {

如果没有初始化本地坐标原点经纬度，则初始化本地坐标原点经纬度

if (!globallocalconverter_initialized()) { 
        
	globallocalconverter_init(_local_pos.ref_lat, _local_pos.ref_lon,
				  _local_pos.ref_alt, _local_pos.ref_timestamp);

}

将全球坐标转为本地坐标

else { 
        
					globallocalconverter_toglobal(_pos_sp_triplet.current.x, _pos_sp_triplet.current.y, _pos_sp_triplet.current.z,
								      &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon, &_pos_sp_triplet.current.alt);

				}
			}

重置计数

// update the reset counters in any case
_pos_reset_counter = _global_pos.lat_lon_reset_counter;

定义位置和速度向量

matrix::Vector3f ground_speed(_local_pos.vx, _local_pos.vy,  _local_pos.vz);
matrix::Vector2f current_position((float)_global_pos.lat, (float)_global_pos.lon);
matrix::Vector3f current_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz);

如果是位置控制模式

if (!manual_mode && _control_mode.flag_control_position_enabled) {

运行位置控制器

if (control_position(current_position, ground_speed, _pos_sp_triplet)) {

赋值相应的状态并发布

	//TODO: check if radius makes sense here
	float turn_distance = _param_l1_distance.get(); //_gnd_control.switch_distance(100.0f);

	// publish status
	position_controller_status_s pos_ctrl_status = { 
        };

	pos_ctrl_status.nav_roll = 0.0f;
	pos_ctrl_status.nav_pitch = 0.0f;
	pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing();

	pos_ctrl_status.target_bearing = _gnd_control.target_bearing();
	pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error();

	pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
				  _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);

	pos_ctrl_status.acceptance_radius = turn_distance;
	pos_ctrl_status.yaw_acceptance = NAN;

	pos_ctrl_status.timestamp = hrt_absolute_time();

	_pos_ctrl_status_pub.publish(pos_ctrl_status);


}

如果是速度控制模式，则运行速度控制器

} else if (!manual_mode && _control_mode.flag_control_velocity_enabled) { 
        
	control_velocity(current_velocity, _pos_sp_triplet);
}
			perf_end(_loop_perf);
		}

如果姿态更新并且是使能了姿态控制，则运行姿态控制器

if (fds[3].revents & POLLIN) { 
        

	vehicle_attitude_poll();

	if (!manual_mode && _control_mode.flag_control_attitude_enabled
	    && !_control_mode.flag_control_position_enabled
	    && !_control_mode.flag_control_velocity_enabled) { 
        

		control_attitude(_vehicle_att, _att_sp);

	}

}

订阅遥控器值

if (fds[1].revents & POLLIN) { 
        

	// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
	// returning immediately and this loop will eat up resources.
	orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint);

如果是遥控模式，则直接根据遥控器输出，这里虽然赋值四个，但只有偏航和推力有效

	if (manual_mode) { 
        
		/* manual/direct control */
		//PX4_INFO("Manual mode!");
		_act_controls.control[actuator_controls_s::INDEX_ROLL] = _manual_control_setpoint.y;
		_act_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual_control_setpoint.x;
		_act_controls.control[actuator_controls_s::INDEX_YAW] = _manual_control_setpoint.r; //TODO: Readd yaw scale param
		_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z;
	}
}

订阅传感器数据

if (fds[2].revents & POLLIN) { 
        

	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);

只要有任何一种控制方式使能，就发布控制量

		//orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_vehicle_att);
		_act_controls.timestamp = hrt_absolute_time();

		/* Only publish if any of the proper modes are enabled */
		if (_control_mode.flag_control_velocity_enabled ||
		    _control_mode.flag_control_attitude_enabled ||
		    _control_mode.flag_control_position_enabled ||
		    manual_mode) { 
        
			/* publish the actuator controls */
			_actuator_controls_pub.publish(_act_controls);
		}
	}

}

取消订阅

	orb_unsubscribe(_control_mode_sub);
	orb_unsubscribe(_global_pos_sub);
	orb_unsubscribe(_local_pos_sub);
	orb_unsubscribe(_manual_control_setpoint_sub);
	orb_unsubscribe(_pos_sp_triplet_sub);
	orb_unsubscribe(_vehicle_attitude_sub);
	orb_unsubscribe(_sensor_combined_sub);

	warnx("exiting.\n");
}

二、位置控制

bool
RoverPositionControl::control_position(const matrix::Vector2f &current_position,
				       const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
{

控制器的运行时间间隔，使用非零值避免被零除

float dt = 0.01; // Using non zero value to a avoid division by zero

更新dt

if (_control_position_last_called > 0) { 
        
	dt = hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
}
	_control_position_last_called = hrt_absolute_time();

bool setpoint = true;

如果是auto或者offboard模式，并且存在期望位置

if ((_control_mode.flag_control_auto_enabled ||
     _control_mode.flag_control_offboard_enabled) && pos_sp_triplet.current.valid) {

赋值控制模式

_control_mode_current = UGV_POSCTRL_MODE_AUTO;

期望经纬度航点向量

/* current waypoint (the one currently heading for) */
matrix::Vector2f curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);

如果存在上一个航点，则更新上一个航点，运行L1算法必须知道下一个航点和上一个航点

/* previous waypoint */
matrix::Vector2f prev_wp = curr_wp;

if (pos_sp_triplet.previous.valid) { 
        
	prev_wp(0) = (float)pos_sp_triplet.previous.lat;
	prev_wp(1) = (float)pos_sp_triplet.previous.lon;
}

定义地速向量

matrix::Vector2f ground_speed_2d(ground_speed);

定义推力

float mission_throttle = _param_throttle_cruise.get();

	/* Just control the throttle */
	if (_param_speed_control_mode.get() == 1) {
		/* control the speed in closed loop */

获取巡航速度

float mission_target_speed = _param_gndspeed_trim.get();

if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) &&
    _pos_sp_triplet.current.cruising_speed > 0.1f) { 
        
	mission_target_speed = _pos_sp_triplet.current.cruising_speed;
}

将地速转换到机体坐标系

// Velocity in body frame
const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2));

指向机头的速度

const float x_vel = vel(0);

指向机头的加速度

const float x_acc = _vehicle_acceleration_sub.get().xyz[0];

PID计算期望推力

// Compute airspeed control out and just scale it as a constant
mission_throttle = _param_throttle_speed_scaler.get()
		   * pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt);

推力限幅

// Constrain throttle between min and max
mission_throttle = math::constrain(mission_throttle, _param_throttle_min.get(), _param_throttle_max.get());

如果在_pos_sp_triplet中指定了期望推力，则使用开换控制

} else { 
        
	/* Just control throttle in open loop */
	if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_throttle) &&
	    _pos_sp_triplet.current.cruising_throttle > 0.01f) { 
        

		mission_throttle = _pos_sp_triplet.current.cruising_throttle;
	}
}

获取当前位置到下一个航点的距离

float dist_target = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
		    (double)curr_wp(0), (double)curr_wp(1)); // pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);

如果是跑航点

switch (_pos_ctrl_state) { 
        
case GOTO_WAYPOINT: {

如果当前点到下一个航点的距离小于参数NAV_LOITER_RAD，则进入停止

if (dist_target < _param_nav_loiter_rad.get()) { 
        
	_pos_ctrl_state = STOPPING;  // We are closer than loiter radius to waypoint, stop.

}

利用L1算法计算侧向加速度

else { 
        
					_gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);

赋值推力

_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;

根据侧向加速度计算偏航力矩．无人车/船在施加偏航力矩后，实际上是在做圆周运动，根据圆周运动加速度计算公式：a=v^2/r 其中期望加速度a和运动速度v已知，可以求出期望的转弯半径r＝v^2/a,这里只计算绝对值．

float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand());

根据几何知识，可以由转弯半径和车辆前轴到后轴的距离_param_wheel_base（参数名称GND_WHEEL_BASE）计算转舵的角度

float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get());

根据转舵角度结合侧向加速度的方向，计算出偏航控制量大小和方向

float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign(
　_gnd_control.nav_lateral_acceleration_demand());
control_effort = math::constrain(control_effort, -1.0f, 1.0f);

输出

_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
	}
}
	break;

停止状态偏航和推力都为0，

case STOPPING: { 
        
		_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
		_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;

求上一个航点与下一个航点的距离

// Note _prev_wp is different to the local prev_wp which is related to a mission waypoint.
float dist_between_waypoints = get_distance_to_next_waypoint((double)_prev_wp(0), (double)_prev_wp(1),
			       (double)curr_wp(0), (double)curr_wp(1));

如果距离大于0,说明有新的航点，进入航点状态

if (dist_between_waypoints > 0) { 
        
	_pos_ctrl_state = GOTO_WAYPOINT; // A new waypoint has arrived go to it
}
			}
			break;

默认置为停止状态

default:
	PX4_ERR("Unknown Rover State");
	_pos_ctrl_state = STOPPING;
	break;
}

当前航点置为上一次的航点，

_prev_wp = curr_wp;

	} else { 
        
		_control_mode_current = UGV_POSCTRL_MODE_OTHER;
		setpoint = false;
	}

	return setpoint;
}

三、速度控制

void
RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity,
				       const position_setpoint_triplet_s &pos_sp_triplet)
{

控制器的运行时间间隔，使用非零值避免被零除

float dt = 0.01; // Using non zero value to a avoid division by zero

定义期望推力

const float mission_throttle = _param_throttle_cruise.get();

从pos_sp_triplet中取期望速度

const matrix::Vector3f desired_velocity{ 
        pos_sp_triplet.current.vx, pos_sp_triplet.current.vy, pos_sp_triplet.current.vz};

期望速度取模

const float desired_speed = desired_velocity.norm();

将速度转换到机体坐标系

if (desired_speed > 0.01f) { 
        
	const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
	const Vector3f vel = R_to_body * Vector3f(current_velocity(0), current_velocity(1), current_velocity(2));

定义机头方向的速度和家速度


        标签： px3981传感器
 
锐单商城拥有海量元器件数据手册、
          IC替代型号，打造
          电子元器件IC百科大全！

锐单商城 - 一站式电子元器件采购平台