晴雨表和GPS实施起来相对简单，但不够精确和缓慢。对于加速度计读数，我通过一个低通滤波器消除了恒定的9.81 m /s²加速度。然后，我将这些数据用于计算爬升率（以厘米/秒为单位）。我知道这种方式的速度逼近不是很好。但是，到目前为止，我还没有更好的方法。
为了计算电动机速度，我使用了atm两个PID（STAB和RATE）。
我编写了下面显示的示例，到目前为止没有进行大量测试。我相信它将无法顺利顺利地解决。例如可以使用气压计的爬升率代替加速度计的计算速度。但是对于低海拔和小变化，我确实很可能需要加速度计。
ArduPilot似乎以不同的方式使用了第三个PID进行加速。我相信他们像我一样计算身高差。然后他们将STAB-Pid用作气压计的爬升率（不像我的加速度计），并用加速度数据计算另一个输出。不幸的是，我不知道该怎么精确，或者是否还有其他方法。
有人知道气压计和加速度计实现高度保持功能的确切布局。我的意思是我真的不确定我的想法是否正确。也许以后我可以发布一些选项。
我的PID：
m_rgPIDS[PID_THR_STAB].kP(1.25); // For altitude hold
m_rgPIDS[PID_THR_RATE].kP(0.35); // For altitude hold
m_rgPIDS[PID_THR_RATE].kI(0.15); // For altitude hold
m_rgPIDS[PID_THR_RATE].imax(100); // For altitude hold
保持高度的代码：
// Stabilizing code done before
float fCurAlti_cm = _HAL_BOARD.get_alti_m() * 100.f; // Barometer and GPS data
float fAcclClimb_cms = _HAL_BOARD.get_accel_mg_ms().z * 100; // Accelerometer output in cm per s (gravitational const. corrected)
// calculate the difference between current altitude and altitude wanted
float fAltStabOut = _HAL_BOARD.m_rgPIDS[PID_THR_STAB].get_pid(fCurAlti_cm - (float)(rcalt_m*100), 1);
// Rate it with climb rate (here with accelerometer)
int_fast16_t iAltOutput = _HAL_BOARD.m_rgPIDS[PID_THR_RATE].get_pid(fAltStabOut - fAcclClimb_cms, 1);
// Modify the speed of the motors
iFL += iAltOutput;
iBL += iAltOutput;
iFR += iAltOutput;
iBR += iAltOutput;
#1 楼
抱歉，我不确定您的代码到底在做什么，但总的来说... ...而不是直接尝试使用加速度计进行控制，通常最好估算速度并使用它进行控制（如PID控制器的D部分）。如果您想深入研究数学，最常用的方法是使用卡尔曼滤波器将气压计和加速度计的测量结果结合起来。这将为您提供高度和速度的平滑估计，然后可以将其用作姿态控制器的P和D。
#2 楼
我为此做了一个小例行程序，但还没有完全没有错误。例如，加速度项仍与高度项的差异相矛盾，但要很好地发挥作用。const float fBias_g = 0.1f;
const float fScaleF_g = 100.0f;
int_fast16_t iAltZOutput = 0; // Barometer & Sonar
int_fast16_t iAccZOutput = 0; // Accelerometer
// Return estimated altitude by GPS and barometer
bool bOK_H, bOK_R, bOK_G;
int_fast32_t fCurAlti_cm = altitude_cm(&_HAL_BOARD, bOK_H);
float fClimbRate_cms = climbrate_cms(&_HAL_BOARD, bOK_R);
// Get the acceleration in g
Vector3f vAccel_g = accel_g(&_HAL_BOARD, bOK_G) * fScaleF_g;
// Calculate the motor speed changes by the error from the height estimate and the current climb rates
// If the quadro is going down, because of an device error, then this code is not used
if(_RECVR.m_Waypoint.mode != GPSPosition::CONTRLD_DOWN_F) {
float fAltZStabOut = _HAL_BOARD.m_rgPIDS[PID_THR_STAB].get_pid((float)(rcalt_cm - fCurAlti_cm), 1);
iAltZOutput = _HAL_BOARD.m_rgPIDS[PID_THR_RATE].get_pid(fAltZStabOut - fClimbRate_cms, 1);
}
// Don't change the throttle if acceleration is below a certain bias
vAccel_g.z = sign_f(vAccel_g.z) * (abs(vAccel_g.z) - fBias_g) * fScaleF_g;
// Calculate the throttle changes if copter changes altitude
float fAccZStabOut = _HAL_BOARD.m_rgPIDS[PID_ACC_STAB].get_pid(vAccel_g.z, 1);
iAccZOutput = _HAL_BOARD.m_rgPIDS[PID_ACC_RATE].get_pid(fAccZStabOut, 1);
// Modify the speed of the motors to hold the altitude
iFL += iAltZOutput + iAccZOutput;
iBL += iAltZOutput + iAccZOutput;
iFR += iAltZOutput + iAccZOutput;
iBR += iAltZOutput + iAccZOutput;
评论
$ \ begingroup $
我认为，传感器融合很容易实现。无论如何，我不明白这些加速度PID的工作原理，请在此处进行解释：copter.ardupilot.com/wiki/altholdmode：“油门加速PID PID增益将加速度误差（即所需加速度与实际加速度之差）转换为电动机输出功率，如果修改这些参数，则应保持P与I的1：2比（即I是P大小的两倍），这些值绝不能增加，但对于功率非常大的直升机，可以通过减小两者来获得更好的响应降低50％（即P变为0.5，I变为1.0）。”
$ \ endgroup $
– dgrat
2014年3月23日在11:03
$ \ begingroup $
我的意思是，对于高度保持来说，加速度计数据已经包含在计算中了。
$ \ endgroup $
– dgrat
2014年3月23日在11:04
$ \ begingroup $
可以这样做，但这并不理想。像这样使用加速度计数据的问题在于噪声。四旋翼飞机通常具有低成本的加速度计，当您加上振动时，它们会变得相当嘈杂。这就是为什么大多数研究小组使用特殊算法（例如卡尔曼滤波器）来组合所有传感器测量值，但可以降低噪声的原因。
$ \ endgroup $
–ryan0270
2014年3月23日14:43