First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

增加差速电机前馈与控制器曲率预瞄

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-12 11:19:01 +08:00
parent 3043febd91
commit 6c6149c8d5
24 changed files with 366 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MedullaAdapter/DiverCartDefinition.cs
+18
View File
@@ -64,6 +64,16 @@ namespace MedullaAdapter
#endregion
#region 初始参数
[AsInitParam(desc = "差速转舵目标角速度前馈增益")]
public float DiffSteerRateFeedforwardGain = 0.9f;
[AsInitParam(desc = "差速舵轮左右轮间距，单位mm")]
public float DiffSteerWheelDistanceMillimeters = 85f;
[AsInitParam(desc = "差速转舵前馈最大速度，单位m/s")]
public float DiffSteerRateFeedforwardMaximumSpeed = 0.03f;
[AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
[AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
[AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
@@ -100,6 +110,14 @@ namespace MedullaAdapter
[IOObjectMonitor(desc = "左后舵轮转向PID输出")] public float DiffSteerOutputLeftRear;
[IOObjectMonitor(desc = "右前舵轮转向PID输出")] public float DiffSteerOutputRightFront;
[IOObjectMonitor(desc = "右后舵轮转向PID输出")] public float DiffSteerOutputRightRear;
[IOObjectMonitor(desc = "左前舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardLeftFront;
[IOObjectMonitor(desc = "左后舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardLeftRear;
[IOObjectMonitor(desc = "右前舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardRightFront;
[IOObjectMonitor(desc = "右后舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardRightRear;
[IOObjectMonitor(desc = "左前舵轮转向合成差速输出")] public float DiffSteerTotalOutputLeftFront;
[IOObjectMonitor(desc = "左后舵轮转向合成差速输出")] public float DiffSteerTotalOutputLeftRear;
[IOObjectMonitor(desc = "右前舵轮转向合成差速输出")] public float DiffSteerTotalOutputRightFront;
[IOObjectMonitor(desc = "右后舵轮转向合成差速输出")] public float DiffSteerTotalOutputRightRear;
[IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
[IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f;
[IOObjectMonitor(desc = "轮速诊断记录已启用")]
MedullaAdapter/MotorRoutine.cs
+165 -12
View File
@@ -3,14 +3,22 @@ using CartActivator;
using FundamentalLib;
using MDCSToolBox.Commons;
using System;
using System.Diagnostics;
using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
namespace MedullaAdapter
{
public class MotorRoutine : LadderLogic<DiverCartDefinition>
{
private const double MaximumFeedforwardIntervalSeconds = 0.2;
private bool _wasTransmitterControlling;
private DateTime _lastMoveTime = DateTime.Now;
private bool _diffSteerFeedforwardInitialized;
private long _lastDiffSteerFeedforwardTimestamp;
private float _previousThLeftFront;
private float _previousThLeftRear;
private float _previousThRightFront;
private float _previousThRightRear;
private DiverCartDefinition.ManualControlMode?
_pendingTransmitterControlMode;
private DateTime _pendingTransmitterControlModeSince =
@@ -213,7 +221,9 @@ namespace MedullaAdapter
cart.SpeedRightArm = 0;
}
// M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
/// <summary>
/// 根据四个舵轮的目标角速度前馈和实际角度反馈修正八个驱动电机速度。
/// </summary>
private void UpdateDiffSteerWheelSpeeds()
{
if (cart.LeftFrontPid == null ||
@@ -229,6 +239,7 @@ namespace MedullaAdapter
cart.SpeedLRR = 0;
cart.SpeedRRL = 0;
cart.SpeedRRR = 0;
ResetDiffSteerRateFeedforward();
return;
}
@@ -272,26 +283,45 @@ namespace MedullaAdapter
cart.DiffSteerThresh,
cart.DiffSteerSpeedAcc);
// 根据实际舵角计算四条腿的差速修正量。
var diffLf = cart.LeftFrontPid.GetResponse(
// 根据实际舵角计算四条腿的PID反馈修正量。
var feedbackLf = cart.LeftFrontPid.GetResponse(
cart.ThLeftFront, false, false, "LF");
var diffLr = cart.LeftRearPid.GetResponse(
var feedbackLr = cart.LeftRearPid.GetResponse(
cart.ThLeftRear, false, false, "LR");
var diffRf = cart.RightFrontPid.GetResponse(
var feedbackRf = cart.RightFrontPid.GetResponse(
cart.ThRightFront, false, false, "RF");
var diffRr = cart.RightRearPid.GetResponse(
var feedbackRr = cart.RightRearPid.GetResponse(
cart.ThRightRear, false, false, "RR");
// 保存四个转向PID的本周期修正量，供M层监控和舵轮响应CSV记录使用。
cart.DiffSteerOutputLeftFront = diffLf;
cart.DiffSteerOutputLeftRear = diffLr;
cart.DiffSteerOutputRightFront = diffRf;
cart.DiffSteerOutputRightRear = diffRr;
CalculateDiffSteerRateFeedforward(
out var feedforwardLf,
out var feedforwardLr,
out var feedforwardRf,
out var feedforwardRr);
// 左前腿：左右电机施加方向相反的PID修正量。
var diffLf = feedbackLf + feedforwardLf;
var diffLr = feedbackLr + feedforwardLr;
var diffRf = feedbackRf + feedforwardRf;
var diffRr = feedbackRr + feedforwardRr;
// 分别保留PID、前馈和合成差速，便于独立标定与诊断。
cart.DiffSteerOutputLeftFront = feedbackLf;
cart.DiffSteerOutputLeftRear = feedbackLr;
cart.DiffSteerOutputRightFront = feedbackRf;
cart.DiffSteerOutputRightRear = feedbackRr;
cart.DiffSteerRateFeedforwardLeftFront = feedforwardLf;
cart.DiffSteerRateFeedforwardLeftRear = feedforwardLr;
cart.DiffSteerRateFeedforwardRightFront = feedforwardRf;
cart.DiffSteerRateFeedforwardRightRear = feedforwardRr;
cart.DiffSteerTotalOutputLeftFront = diffLf;
cart.DiffSteerTotalOutputLeftRear = diffLr;
cart.DiffSteerTotalOutputRightFront = diffRf;
cart.DiffSteerTotalOutputRightRear = diffRr;
// 左前腿：左右电机施加方向相反的合成差速修正量。
cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
@@ -308,6 +338,129 @@ namespace MedullaAdapter
cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
}
/// <summary>
/// 根据四个机械目标舵角的实际变化率计算本周期差速轮线速度前馈。
/// </summary>
private void CalculateDiffSteerRateFeedforward(
out float leftFront,
out float leftRear,
out float rightFront,
out float rightRear)
{
leftFront = 0f;
leftRear = 0f;
rightFront = 0f;
rightRear = 0f;
var currentTimestamp = Stopwatch.GetTimestamp();
if (_diffSteerFeedforwardInitialized)
{
var deltaTimeSeconds =
(currentTimestamp -
_lastDiffSteerFeedforwardTimestamp) /
(double)Stopwatch.Frequency;
if (deltaTimeSeconds > 0.0 &&
deltaTimeSeconds <=
MaximumFeedforwardIntervalSeconds)
{
leftFront = CalculateDiffSteerRateFeedforward(
cart.ThLeftFront,
_previousThLeftFront,
deltaTimeSeconds);
leftRear = CalculateDiffSteerRateFeedforward(
cart.ThLeftRear,
_previousThLeftRear,
deltaTimeSeconds);
rightFront = CalculateDiffSteerRateFeedforward(
cart.ThRightFront,
_previousThRightFront,
deltaTimeSeconds);
rightRear = CalculateDiffSteerRateFeedforward(
cart.ThRightRear,
_previousThRightRear,
deltaTimeSeconds);
}
}
_previousThLeftFront = cart.ThLeftFront;
_previousThLeftRear = cart.ThLeftRear;
_previousThRightFront = cart.ThRightFront;
_previousThRightRear = cart.ThRightRear;
_lastDiffSteerFeedforwardTimestamp = currentTimestamp;
_diffSteerFeedforwardInitialized = true;
}
/// <summary>
/// 将单个机械目标舵角变化率转换为带限幅的左右轮差速线速度前馈。
/// </summary>
private float CalculateDiffSteerRateFeedforward(
float targetAngleDegrees,
float previousTargetAngleDegrees,
double deltaTimeSeconds)
{
var gain = cart.DiffSteerRateFeedforwardGain;
var wheelDistanceMillimeters =
cart.DiffSteerWheelDistanceMillimeters;
var maximumSpeed =
cart.DiffSteerRateFeedforwardMaximumSpeed;
if (!IsFinite(gain) || gain <= 0f ||
!IsFinite(wheelDistanceMillimeters) ||
wheelDistanceMillimeters <= 0f ||
!IsFinite(maximumSpeed) || maximumSpeed <= 0f ||
!IsFinite(targetAngleDegrees) ||
!IsFinite(previousTargetAngleDegrees))
{
return 0f;
}
// 机械舵角受限，必须使用直接差值而不是圆周最短角差。
var targetRateRadiansPerSecond =
(targetAngleDegrees - previousTargetAngleDegrees) *
Math.PI / 180.0 /
deltaTimeSeconds;
var wheelDistanceMeters =
wheelDistanceMillimeters / 1000.0;
var feedforwardSpeed =
0.5 *
wheelDistanceMeters *
targetRateRadiansPerSecond *
gain;
return (float)Math.Clamp(
feedforwardSpeed,
-maximumSpeed,
maximumSpeed);
}
/// <summary>
/// 清除差速转舵前馈历史和监控输出，避免恢复控制时使用过期目标角。
/// </summary>
private void ResetDiffSteerRateFeedforward()
{
_diffSteerFeedforwardInitialized = false;
_lastDiffSteerFeedforwardTimestamp = 0;
cart.DiffSteerRateFeedforwardLeftFront = 0f;
cart.DiffSteerRateFeedforwardLeftRear = 0f;
cart.DiffSteerRateFeedforwardRightFront = 0f;
cart.DiffSteerRateFeedforwardRightRear = 0f;
cart.DiffSteerTotalOutputLeftFront = 0f;
cart.DiffSteerTotalOutputLeftRear = 0f;
cart.DiffSteerTotalOutputRightFront = 0f;
cart.DiffSteerTotalOutputRightRear = 0f;
}
/// <summary>
/// 判断单精度参数是否可安全参与底盘控制计算。
/// </summary>
private static bool IsFinite(float value)
{
return !float.IsNaN(value) &&
!float.IsInfinity(value);
}
// M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。
private void UpdateSendSpeedLimit()
{
MedullaAdapter/WheelSpeedDiagnosticLogger.cs
+14
View File
@@ -84,7 +84,10 @@ namespace MedullaAdapter
_snapshotWriter.WriteLine(
"ElapsedMs,CarNum,ManualControlMode,ManualMode,SendThresSpeed," +
"DiffSteerKp,DiffSteerKi,DiffSteerKd,DiffSteerMaxI,DiffSteerDeadZone,DiffSteerThresh,DiffSteerSpeedAcc," +
"DiffSteerRateFeedforwardGain,DiffSteerWheelDistanceMillimeters,DiffSteerRateFeedforwardMaximumSpeed," +
"PidOutLeftFront,PidOutLeftRear,PidOutRightFront,PidOutRightRear," +
"RateFeedforwardLeftFront,RateFeedforwardLeftRear,RateFeedforwardRightFront,RateFeedforwardRightRear," +
"TotalDiffLeftFront,TotalDiffLeftRear,TotalDiffRightFront,TotalDiffRightRear," +
"CmdLFL,CmdLFR,CmdLRL,CmdLRR,CmdRFL,CmdRFR,CmdRRL,CmdRRR," +
"PidLFL,PidLFR,PidLRL,PidLRR,PidRFL,PidRFR,PidRRL,PidRRR," +
"ActualLFL,ActualLFR,ActualLRL,ActualLRR,ActualRFL,ActualRFR,ActualRRL,ActualRRR," +
@@ -217,10 +220,21 @@ namespace MedullaAdapter
Format(cart.DiffSteerDeadZone),
Format(cart.DiffSteerThresh),
Format(cart.DiffSteerSpeedAcc),
Format(cart.DiffSteerRateFeedforwardGain),
Format(cart.DiffSteerWheelDistanceMillimeters),
Format(cart.DiffSteerRateFeedforwardMaximumSpeed),
Format(cart.DiffSteerOutputLeftFront),
Format(cart.DiffSteerOutputLeftRear),
Format(cart.DiffSteerOutputRightFront),
Format(cart.DiffSteerOutputRightRear),
Format(cart.DiffSteerRateFeedforwardLeftFront),
Format(cart.DiffSteerRateFeedforwardLeftRear),
Format(cart.DiffSteerRateFeedforwardRightFront),
Format(cart.DiffSteerRateFeedforwardRightRear),
Format(cart.DiffSteerTotalOutputLeftFront),
Format(cart.DiffSteerTotalOutputLeftRear),
Format(cart.DiffSteerTotalOutputRightFront),
Format(cart.DiffSteerTotalOutputRightRear),
Format(cart.SpeedLeftFrontLeft),
Format(cart.SpeedLeftFrontRight),
Format(cart.SpeedLeftRearLeft),
MedullaAdapter/build/Medulla/plugins/CommonUsage.dll
BIN
View File
Binary file not shown.
MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll
BIN
View File
Binary file not shown.
MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb
BIN
View File
Binary file not shown.