多旋翼无人机

        与姿态控制类似的，位置控制同样是无人机控制系统中非常重要的控制系统。位置控制实际上就是用于控制无人机在控制飞行的位置。我们希望可以准确的控制无人机按预期的速度飞往指定的目标位置，这也是无人机全自动飞行的基础。为了确定无人机的位置，需要使用在本地坐标系下的x、y、z这3个维度的坐标。但是在控制方面上往往将x轴和y轴上的控制方法称为水平位置控制，而将z轴上的坐标控制方法称为高度控制。但是这里两个控制方法需要同时进行，最终达到对3维坐标系内无人机准确飞行控制目的。

        同样的，由于无人机测量系统对速度较位置更为敏感，因此位置控制也采用了双环反馈控制方法。其内环为速度控制环，外环为位置控制环。如下图所示：

        位置期望：包括x、y、z这3个轴的坐标，也就是系统希望无人机所到达的一个位置。这个位置可以是有无人机自行读取的任务点坐标，也可以使地面站向无人机所指定的坐标，或者是在定点飞行模式中无人机当前的位置。例如我们希望无人机达到某一个坐标点，于是这个坐标点就可以作为位置期望交给位置控制器。

        测量位置：为无人机当前的位置，同样也是在本地3维坐标系中的坐标。测量位置实际上也不是直接通过传感器所得到的，而是通过卡尔曼滤波所得到位置的最优估计值。这个位置的最优估计在控制系统中被作为一个准确值，也就是可以表示控制系统中测量位置的结果。测量位置将佟位置期望一起交给位置控制器。

        位置控制器：外环P控制器，其输入内容为位置期望减去测量位置，也就是位置误差。然后通过“比例”控制方法对位置误差进行计算，得到其输出内容。需要注意的是，位置控制器的输入为位置误差，而输出却是速度期望。也就是系统希望达到这样的速度值，并且保持稳定。

        速度期望：外环控制器的输出，并与内环测量速度计算得到速度误差，并作为内环控制器的输入。

        测量速度：测量角速度为无人机当前的飞行速度，其来源为卡尔曼滤波器得到的最优估计值。

        速度控制器：内环PID控制器，其输入内容为速度期望减去测量速度，得到速度误差。然后通过“比例-积分-微分”控制方法对速度误差进行计算，得到其输出内容。速度控制器的输入为速度期望，而输出却是姿态角期望。

        姿态期望：是速度控制的输出值，这个姿态角期望最为姿态控制的输入。也就是说姿态期望就是位置控制的输出结果，这个结果交给姿态控制，使无人机达到相应的姿态期望，最后通过姿态控制器得到最后相应的控制量。

        整个位置和姿态控制配合的控制系统我们同样可以将其看作是一个串级反馈控制系统。其外环为位置控制，其内环为姿态控制，如下图所示：

        其中位置控制系统中包含“位置-速度”双环控制器，姿态控制系统中包含“角度-角速度”双环控制器。

        接下来我们来看看位置控制的相关代码：

ParamFloat _z_p,                                //z轴比例参数
ParamFloat _z_vel_p,                            //z轴速度比例参数
ParamFloat _z_vel_i,                            //z轴速度积分参数
ParamFloat _z_vel_d,                            //z轴速度微分参数
ParamFloat _xy_p,                               //水平位置比例参数
ParamFloat _xy_vel_p,                           //水平速度比例参数
ParamFloat _xy_vel_i,                           //水平速度积分参数
ParamFloat _xy_vel_d,                           //水平速度微分参数

matrix::Vector3f _pos_p;                        //位置比例参数向量
matrix::Vector3f _vel_p;                        //速度比例参数向量
matrix::Vector3f _vel_i;                        //速度积分参数向量
matrix::Vector3f _vel_d;                        //速度微分参数向量

matrix::Vector3f _pos;                          //当前位置（位置测量）
matrix::Vector3f _pos_sp;                       //位置期望
matrix::Vector3f _vel;                          //当前速度（速度测量）
matrix::Vector3f _vel_sp;                       //速度期望

//更新当前位置控制的所有参数
void parameters_update()
{
	//位置控制的3个比例参数。
	_pos_p(0) = _xy_p.get();
	_pos_p(1) = _xy_p.get();
	_pos_p(2) = _z_p.get();

	//速度控制的3个比例参数、3个积分参数、3个微分参数。
	_vel_p(0) = _xy_vel_p.get();
	_vel_p(1) = _xy_vel_p.get();
	_vel_p(2) = _z_vel_p.get();
	_vel_i(0) = _xy_vel_i.get();
	_vel_i(1) = _xy_vel_i.get();
	_vel_i(2) = _z_vel_i.get();
	_vel_d(0) = _xy_vel_d.get();
	_vel_d(1) = _xy_vel_d.get();
	_vel_d(2) = _z_vel_d.get();
}

//外环位置控制
void calculate_velocity_setpoint()
{
	if (_run_pos_control)
	{
		//位置期望减去当前位置得到位置误差，再乘以比例控制参数，最后得到水平方向上的速度期望。
		_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _pos_p(0);
		_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _pos_p(1);
	}

	//其他代码略
}

void calculate_thrust_setpoint()
{
	//速度期望_vel_sp减去当前速度_vel得到速度误差vel_err。
	matrix::Vector3f vel_err = _vel_sp - _vel;

	matrix::Vector3f thrust_sp;
	//比例、积分、微分控制得到矢量力期望，其中_thrust_int为积分项向量。
	thrust_sp = vel_err.emult(_vel_p) + _thrust_int + _vel_err_d.emult(_vel_d);

	//进行矢量力的积分项计算。
	_thrust_int(0) += vel_err(0) * _vel_i(0) * _dt;
	_thrust_int(1) += vel_err(1) * _vel_i(1) * _dt;
	_thrust_int(2) += vel_err(2) * _vel_i(2) * _dt;

	//定义了x、y、z轴的期望向量。
	matrix::Vector3f body_x;
	matrix::Vector3f body_y;
	matrix::Vector3f body_z;

	//矢量力的归一化得到z轴期望向量，注意此处由于矢量力与z轴期望向量的方向相反
	body_z = -thrust_sp.normalized();
	//y_C为水平方向上航向向量为旋转90度
	matrix::Vector3f y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);

	//通过z轴的期望向量body_z与x轴的期望向量body_x进行叉乘的到y轴的向量期望body_t
	body_x = y_C % body_z;
	//归一化处理
	body_x.normalize();

	//z轴的期望向量body_z与x轴的期望向量body_x进行叉乘的到y轴的向量期望body_t
	body_y = body_z % body_x;
	//归一化处理
	body_y.normalize();

	//填充旋转矩阵_R_setpoint
	for (int i = 0; i < 3; i++)
	{
		_R_setpoint(i, 0) = body_x(i);
		_R_setpoint(i, 1) = body_y(i);
		_R_setpoint(i, 2) = body_z(i);
	}
	//旋转矩阵得到姿态期望的四元数
	matrix::Quatf q_sp = _R_setpoint;
	q_sp.copyTo(_att_sp.q_d);

	//其他代码略
}