PX4飞控源码L1制导律详解

PX4飞控源码L1制导律详解

本文目的在于帮助大家看清楚L1制导律选择参考点的策略，所以作者将与L1知道无关的代码添加删除线。 所有以下划线开头的变量在PX4中都是全局变量， 在下面的函数中， 有_target_bearing //飞机和期望航点的朝向,被用在别的函数中了。  

_L1_distance //根据L1_PERIOD和L1_DAMPING算出来的飞机到L1点的长度。
_crosstrack_error //交叉轨迹的误差,固定翼走曲线路径时用到这个轨迹误差。
_nav_bearing //飞机的导航朝向, 也就是飞机和L1航点的朝向。这个值仅仅是发给地面站做显示，并没有被参与飞控控制。
_lateral_accel //飞机的横向加速度， 这个变量用来计算飞机的横滚角。
_bearing_error 航向的误差，直飞时就等于eta， //这个变量并没有参与飞控控制，也没有被发给地面站做显示。
PX4固件中L1制导律被用在了navigate_loiter() navigate_waypoint() navigate_heading()等4个函数中。
其中navigate_loiter(A点，飞机当前位置，圆的半径，绕圈飞行方向，速度)使用L1控制律飞圆形轨迹。
其中navigate_waypoint(A点，B点，飞机当前位置，速度)使用L1控制律飞曲线轨迹。
其中navigate_heading(期望朝向，飞机当前朝向，速度)使用L1控制飞航向。
意外发现： 假如你想让飞机朝着一个方向平飞，那么你只需执行navigate_level_flight(当前朝向)， 就可以了。

 //L1制导律中很重要的一点就是要处理飞机和路径的关系， 说到路径，大家都知道两点才能形成一个路径。 而两点形成的路径平面可能在飞机前，可能在飞机后，也可能在路径形成的平面上。 
void
ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector2d &vector_B,
		const Vector2d &vector_curr_position, const Vector2f &ground_speed_vector)
{
	float eta = 0.0f;//本函数的目的就是求得eta，飞机飞行速度方向和l1航点之间的夹角。 这个角度会被用于计算横滚角。 

	//计算期望航点的期望航向值，这个_target_bearing是固件的全局变量,也叫目标方位角（这里要注意和导航方位角相区分），被其它进程使用， 本函数再往下没有再用到该变量 
	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0),
			  vector_B(1));

	//强制执行0.1 m/s的最小地面速度，以避免奇点，也就是令系统不能正常工作的点。 
	float ground_speed = math::max(ground_speed_vector.length(), 0.1f);

	//依据期望的时间周期去计算L1的长度
	_L1_distance = _L1_ratio * ground_speed; 
	//_l1_ratio怎么计算的？答_L1_ratio=1.0f/PI*_L1_damping * _L1_period

	//计算从A到B的矢量
	Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
	//检查航路点是否重叠，如果是,跳过A点直接到B点。

	if (vector_AB.length() < 1.0e-6f) {
		vector_AB = get_local_planar_vector(vector_curr_position, vector_B);
	}
	vector_AB.normalize(); //变成单位向量
	//计算经纬度坐标系下航点A到当前飞机所在位置的向量(假设为AC向量，C代表飞机现在的位置)
	Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);

	~~// |AC| *sin(AB和AC的夹角) 为偏离航向的距离
	/* 计算交叉轨迹误差 (仅输出) */ 
	_crosstrack_error = vector_AB % vector_A_to_airplane;~~ 

	 //如果当前飞机在A点后方+-135度角度范围内，而且飞机到A点的距离大于L1的值，那么使用A点作为L1点,如果飞机现在在A和B航点之间，那么采用正常的L1算法逻辑。 
	float distance_A_to_airplane = vector_A_to_airplane.length(); //计算A点到飞机的距离
	float alongTrackDist = vector_A_to_airplane * vector_AB; //|AC|*cos(AB和AC的夹角)， 也就是AC在AB上的投影

	/* 估计飞机位置 WRT to B */
	Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
    //计算飞机位置矢量相对于line的角度
    
	// XXX this could probably also be based solely on the dot product
	//这有可能仅是基于点积
	float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB);

	/* extension from [2], fly directly to A */
	//|AC|长度大于_L1_distance,并且AC和AB的夹角大于正负135度,将使用A点作为L1参考点。  
	if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane, 1.0f) < -0.7071f) {
		//飞机飞往A点时，飞机的偏航eta(北东地坐标系下)怎么计算？
		
		Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();// 从A点到飞机当前位置的单位矢量计算
	
		float xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); //垂直AC向量方向的速度
		float ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); //平行于AC向量方向的速度
		eta = atan2f(xtrack_vel, ltrack_vel); //飞机飞行速度方向和L1的夹角
		
		~~//飞机当前位置到L1点的朝向，就是导航方位角
		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));~~ 
		
		//如果AB向量和B到飞机的向量是同一个方向，we have missed the waypoint. At +- 90 degrees we are just passing it.
		//如果飞机超过了B点，那就以B点为L1参考点。  
	} else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) {
		//在从航路点A到航路点B过程中，飞机被切换到了手动模式，然后又回到mission模式， 导致错过了航路点， 也就是飞机本身已经过了B点， 那么这个时候，飞机要飞回到B点。 然后再做打算, 那么飞机的偏航角eta(北东地坐标系下)怎么计算呢。 
		float xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);//垂直AC向量方向的速度.
		float ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);//平行于AC向量方向的速度.
		eta = atan2f(xtrack_vel, ltrack_vel);//飞机飞行速度方向和L1航点的夹角

		~~//飞机当前位置到L1航点的航向，就是导航方位角,但是这个导航方位角在哪里被使用？
		_nav_bearing = atan2f(-vector_B_to_P_unit(1), -vector_B_to_P_unit(0));~~ 
	} else {
		//假如飞机位置在A和B之间，来计算飞机飞行速度向量方向和飞机与L1连接线方向的夹角eta， 采用eat=eta1+eta2来计算。 
		float xtrack_vel = ground_speed_vector % vector_AB;//垂直AC向量方向的速度
		float ltrack_vel = ground_speed_vector * vector_AB;//平行于AC向量方向的速度
		
		//eta2是飞机飞行速度向量方向和与期望路径平行线(经过飞机现在位置)的夹角
		float eta2 = atan2f(xtrack_vel, ltrack_vel); //tann2=d导/V, 这里的V就是平行于AC向量方向的速度。 

		//计算 eta1 (经过飞机当前位置的期望路径的平行线和飞机到L1连线的夹角) 
		float xtrackErr = vector_A_to_airplane % vector_AB;
		float sine_eta1 = xtrackErr / math::max(_L1_distance, 0.1f);//sinn1=d/L1

		//限制输出在45度以内
		//eta1是与期望路径平行线(经过飞机现在位置)和飞机与L1航点连线的夹角
		sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071
		float eta1 = asinf(sine_eta1);
		eta = eta1 + eta2;//当无人机位置在A和B位置中间附近时，飞机的偏航（北东地坐标系下）这么计算。 

		~~// 飞机当前位置到L1点的朝向 就是导航方位角
		_nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1;~~ 
	}
 	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);	//限制角度在90度以内
	//这不就是公式吗？ a=2*v*v/l1(n1+n2), 这个值是全局变量，将被用于update_roll_setpoint（）函数中计算横滚值。
	_lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);

	~~_circle_mode = false;/* 飞到这个航点，这里不是circle模式， 这个全局变量被用在navigate_loiter()函数中 */~~ 
	
	_bearing_error = eta;	/* 北东地坐标系下的飞机朝向角度 */ 这个值，飞控中也没有用到。  

	update_roll_setpoint();  //这个函数就是公式roll=arctan(a/g)， _lateral_accel就是这里的a。  
}

接下来我们看一下navigate_heading()函数，该函数字面意思就是给个方向一直飞。 这个方向是哪个坐标系下，需要研究。

void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading,
		const Vector2f &ground_speed_vector)
{
	 //本函数中命令航向是唯一的参考，不发生交叉轨迹校正的情况， 目标方位角（_target_bearing）和导航方位角(_nav_bearing)变得相同
	_target_bearing = _nav_bearing = wrap_pi(navigation_heading);

	float eta = wrap_pi(_target_bearing - wrap_pi(current_heading));

	//方位角的误差正好就是eta
	_bearing_error = eta; //_crosstrack_error被navigate_level_flight(朝向)函数使用,但是navigate_level_flight(朝向)这个函数也没有被飞控任何模块使用。  

	//地面速度是地面速度矢量的长度
	float ground_speed = ground_speed_vector.length();

	//调整L1的距离来保持恒定的频率
	_L1_distance = ground_speed / _heading_omega;
	float omega_vel = ground_speed * _heading_omega;

	~~_circle_mode = false;/* 不是circle模式，这个全局变量被用在navigate_loiter()函数中 */~~ 

	//导航航向，顾名思义，是指没有交叉航迹错误， _crosstrack_error被navigate_level_flight(朝向)函数使用
	_crosstrack_error = 0; 

	/* 限制eta角度到90度 */
	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
	_lateral_accel = 2.0f * sinf(eta) * omega_vel;

	update_roll_setpoint(); //roll=artan（a/g）这里的a就是上一条的_lateral_accel
}

下面这个函数就是，依据横向角速度计算横滚角。

void ECL_L1_Pos_Controller::update_roll_setpoint()
{
	float roll_new = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); //roll=artan（a/g）
	roll_new = math::constrain(roll_new, -_roll_lim_rad, _roll_lim_rad);

	if (_dt > 0.0f && _roll_slew_rate > 0.0f) {
		// slew rate limiting active
		roll_new = math::constrain(roll_new, _roll_setpoint - _roll_slew_rate * _dt, _roll_setpoint + _roll_slew_rate * _dt);
	}

	if (PX4_ISFINITE(roll_new)) {
		_roll_setpoint = roll_new;
	}

}