64 lines
1.8 KiB
C++
64 lines
1.8 KiB
C++
#include "agv_model.h"
|
||
#include <algorithm>
|
||
|
||
#define _USE_MATH_DEFINES
|
||
#include <cmath>
|
||
|
||
#ifndef M_PI
|
||
#define M_PI 3.14159265358979323846
|
||
#endif
|
||
|
||
AGVModel::AGVModel(double wheelbase, double max_velocity, double max_steering_angle)
|
||
: wheelbase_(wheelbase)
|
||
, max_velocity_(max_velocity)
|
||
, max_steering_angle_(max_steering_angle) {
|
||
}
|
||
|
||
AGVModel::State AGVModel::derivative(const State& state, const Control& control) const {
|
||
State dstate;
|
||
|
||
// 单舵轮AGV运动学方程
|
||
// dx/dt = v * cos(theta)
|
||
// dy/dt = v * sin(theta)
|
||
// dtheta/dt = (v / L) * tan(delta)
|
||
// 其中 L 是轮距，delta 是舵轮转向角
|
||
|
||
dstate.x = control.v * std::cos(state.theta);
|
||
dstate.y = control.v * std::sin(state.theta);
|
||
dstate.theta = (control.v / wheelbase_) * std::tan(control.delta);
|
||
|
||
return dstate;
|
||
}
|
||
|
||
AGVModel::State AGVModel::update(const State& state, const Control& control, double dt) const {
|
||
// 限制控制输入
|
||
Control clamped_control = clampControl(control);
|
||
|
||
// 使用欧拉法更新状态
|
||
State dstate = derivative(state, clamped_control);
|
||
|
||
State new_state;
|
||
new_state.x = state.x + dstate.x * dt;
|
||
new_state.y = state.y + dstate.y * dt;
|
||
new_state.theta = state.theta + dstate.theta * dt;
|
||
|
||
// 归一化角度到 [-π, π]
|
||
while (new_state.theta > M_PI) new_state.theta -= 2.0 * M_PI;
|
||
while (new_state.theta < -M_PI) new_state.theta += 2.0 * M_PI;
|
||
|
||
return new_state;
|
||
}
|
||
|
||
AGVModel::Control AGVModel::clampControl(const Control& control) const {
|
||
Control clamped;
|
||
|
||
// 限制速度
|
||
clamped.v = std::max(-max_velocity_, std::min(max_velocity_, control.v));
|
||
|
||
// 限制转向角
|
||
clamped.delta = std::max(-max_steering_angle_,
|
||
std::min(max_steering_angle_, control.delta));
|
||
|
||
return clamped;
|
||
}
|