系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

文章目录

系列文章目录
前言
一、遥控手柄输入的躯干状态指令类型（位置、姿态、角度、角速度）
- 类型
- 代码
二、四足机器人躯干状态需要的状态类型
- 类型
- 代码
三、遥控手柄状态指令转换为机器人躯干状态类型【核心】
- 1.原理
- 2.代码
四、躯干期望的COM轨迹置0函数（初始化时用）
五、数值死区限制函数（用在遥控手柄摇杆上的线速度、角速度上）
六、实时打印相关状态，用于调试
- （1）实时打印手柄操作的状态指令
- （2）实时打印躯干轨迹的状态
总结

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

本文先对遥控手柄状态指令转换为机器人躯干状态输入做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

提示：以下是本篇文章正文内容

一、遥控手柄输入的躯干状态指令类型（位置、姿态、角度、角速度）

类型

（1）前进线速度
（2）横向线速度
（3）垂直线速度

（4）躯干质心X位置
（5）躯干质心Y位置
（6）躯干Z高度位置

（7）躯干左右横滚角速度WR
（8）躯干前后俯仰角速度WP
（9）躯干偏航角速度WY

（10）躯干左右横滚角度R
（11）躯干前后俯仰角度P【防盗标记–盒子君hzj】
（12）躯干偏航角度Y
这是定义的比较全的，遥感是二维的，一拨就是两个数据

代码

/*** 功能：遥控器期望状态参考COM轨迹数据*/
template <typename T>
struct DesiredStateData
{EIGEN_MAKE_ALIGNED_OPERATOR_NEWDesiredStateData() { zero(); }void zero();                            // 把所有数据归零Vec12<T> stateDes;                      // 瞬时期望状态指令Vec12<T> pre_stateDes;                  //上一次期望状态指令Eigen::Matrix<T, 12, 10> stateTrajDes;  // 期望的未来状态轨迹(最多10个时间步长MPC)
};

.
.

二、四足机器人躯干状态需要的状态类型

类型

（1）躯干前进线速度
（2）躯干横向线速度

（3）躯干质心X位置
（4）躯干质心Y位置【防盗标记–盒子君hzj】

（5）躯干偏航角速度WY
（6）躯干偏航角度Y

代码

/*** 功能：躯干状态参考COM轨迹规划命令*/
template <typename T>
class DesiredStateCommand
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW//使用Gamepad Command结构初始化，fun(游戏手柄命令、手柄参数设置、机器人控制参数、状态估计器数据)DesiredStateCommand(GamepadCommand* command, rc_control_settings* rc_command,RobotControlParameters* _parameters,StateEstimate<T>* sEstimate, float _dt)
{gamepadCommand = command;     //rcCommand = rc_command;       //stateEstimate = sEstimate;    //parameters = _parameters;     //data.stateDes.setZero();      //data.pre_stateDes.setZero();  //leftAnalogStick.setZero();    //rightAnalogStick.setZero();   //dt = _dt;}void convertToStateCommands();void setCommandLimits(T minVelX_in, T maxVelX_in,T minVelY_in, T maxVelY_in, T minTurnRate_in, T maxTurnRate_in);void desiredStateTrajectory(int N, Vec10<T> dtVec);void printRawInfo();void printStateCommandInfo();float deadband(float command, T minVal, T maxVal);// 这些应该来自接口T maxRoll = 0.4;T minRoll = -0.4;T maxPitch = 0.4;T minPitch = -0.4;T maxVelX = 3.0;T minVelX = -3.0;T maxVelY = 2.0;T minVelY = -2.0;T maxTurnRate = 2.5;T minTurnRate = -2.5;Vec2<float> leftAnalogStick;Vec2<float> rightAnalogStick;// 保持瞬时期望状态和未来期望状态轨迹DesiredStateData<T> data;const rc_control_settings* rcCommand;const GamepadCommand* gamepadCommand;bool trigger_pressed = false;private:StateEstimate<T>* stateEstimate;RobotControlParameters* parameters;Mat12<T> A;                 //离散时间近似的动力学矩阵T dt;                       //控制回路时间步长变化T deadbandRegion = 0.075;   //模拟斗杆死区的值切断const T filter = 0.1;int printNum = 5;           //选择每隔N次迭代打印信息的频率// N*(0.001s) in simulation timeint printIter = 0;          // 跟踪自上次信息打印以来的迭代次数
};

.
.

三、遥控手柄状态指令转换为机器人躯干状态类型【核心】

1.原理

通过手柄目标命令+状态估计器当前状态计算想要的躯干状态轨迹（12个元素，实质设置6个元素）【防盗标记–盒子君hzj】

（0）先把上次的期望状态轨迹数据清零

（1）计算并设置前进线速度（主要）

用遥控手柄左上下操纵杆输入的模拟值限幅设置【防盗标记–盒子君hzj】

（2）计算并设置横向线速度（主要）

用遥控手柄左的左右操纵杆输入的模拟值限幅设置

（3）计算并设置垂直线速度

默认没有垂直速度

（4）计算并设置躯干质心X位置（主要）

公式：躯干质心X位置=状态估计器对X位置的估计值+（手柄控制周期*手柄当前设置的躯干X位置）

（5）计算并设置躯干质心Y位置（主要）

公式：躯干质心Y位置=状态估计器对Y位置的估计值+（手柄控制周期*手柄当前设置的躯干Y位置）

（6）计算并设置Z高度位置【防盗标记–盒子君hzj】

机器人的高度是给定的，默认是45CM

（7）计算并设置左右横滚角速度WR

默认左右滚动速度为0

（8）计算并设置前后俯仰角速度WP【防盗标记–盒子君hzj】

默认前后俯仰角速度为0

（9）计算并设置偏航角速度WY（主要）

通过手柄限幅设置当前躯干偏航速度

（10）计算并设置左右横滚角度R

模型简化的时候假设左右滚动角度=0

（11）计算并设置前后俯仰角度P

模型简化的时候假设前后俯仰角度=0

（12）计算并设置偏航角度Y（主要）

公式：偏航角度Yaw=状态估计器对偏航角度Yaw的估计值+（手柄控制周期*手柄当前设置的躯干偏航角度Yaw）

2.代码

/*** 功能：躯干期望的COM轨迹生成【手柄目标命令+状态估计器当前状态】计算想要的状态轨迹（12个状态变量）*/
template <typename T>
void DesiredStateCommand<T>::convertToStateCommands()
{data.zero();    //（0）先把上次的期望状态轨迹数据清零//（1）设置前进线速度//     --用遥控手柄左上下操纵杆输入的模拟值限幅设置data.stateDes(6) =deadband(gamepadCommand->leftStickAnalog[1], minVelX, maxVelX);//遥控手柄左上下操纵杆最大或最小模拟数值死区限制//（2）设置横向线速度//     --用遥控手柄左的左右操纵杆输入的模拟值限幅设置data.stateDes(7) =deadband(gamepadCommand->leftStickAnalog[0], minVelY, maxVelY);//遥控手柄左左右操纵杆最大或最小模拟数值死区限制//（3）设置垂直线速度//     --默认没有垂直速度data.stateDes(8) = 0.0;//（4）设置躯干质心X位置//    --公式：躯干质心X位置=状态估计器对X位置的估计值+（手柄控制周期*手柄当前设置的躯干X位置）           解释：期望的x位置增量=手柄控制周期*手柄当前设置的躯干X位置，原理类似于比例控制器data.stateDes(0) = stateEstimate->position(0) + dt * data.stateDes(6);//（5）设置躯干质心Y位置//    --公式：躯干质心Y位置=状态估计器对Y位置的估计值+（手柄控制周期*手柄当前设置的躯干Y位置）           解释：期望的Y位置增量=手柄控制周期*手柄当前设置的躯干Y位置，原理类似于比例控制器data.stateDes(1) = stateEstimate->position(1) + dt * data.stateDes(7);//（6）设置Z高度位置//    --机器人的高度是给定的，默认是45CMdata.stateDes(2) = 0.45;//（7）设置左右横滚角速度  //     --默认左右滚动速度为0，模型简化的时候假设左右滚动速度=0data.stateDes(9) = 0.0;//（8）设置前后俯仰角速度 //     --默认前后俯仰角速度为0，模型简化的时候假设前后倾斜速度=0data.stateDes(10) = 0.0;//（9）设置偏航角速度//    --通过手柄限幅设置当前躯干偏航速度data.stateDes(11) =deadband(gamepadCommand->rightStickAnalog[0], minTurnRate, maxTurnRate);//（10）设置左右横滚角度  //    --模型简化的时候假设左右滚动角度=0data.stateDes(3) = 0.0;//（11）设置前后俯仰角度//    --模型简化的时候假设前后俯仰角度=0data.stateDes(4) = deadband(gamepadCommand->rightStickAnalog[1], minPitch, maxPitch);//（12）设置偏航角度 //   --公式：偏航角度Yaw=状态估计器对偏航角度Yaw的估计值+（手柄控制周期*手柄当前设置的躯干偏航角度Yaw）           解释：期望的偏航角度Yaw增量=手柄控制周期*手柄当前设置的躯干偏航角度Yaw，原理类似于比例控制器data.stateDes(5) = stateEstimate->rpy(2) + dt * data.stateDes(11);
}

.
.

四、躯干期望的COM轨迹置0函数（初始化时用）

/*** 功能：躯干期望的COM轨迹置0函数（初始化时用）*/
template <typename T>
void DesiredStateData<T>::zero()
{stateDes = Vec12<T>::Zero();stateTrajDes = Eigen::Matrix<T, 12, 10>::Zero();
}template struct DesiredStateData<double>;
template struct DesiredStateData<float>;

五、数值死区限制函数（用在遥控手柄摇杆上的线速度、角速度上）

/***  功能：数值死区限制函数，用在遥控手柄摇杆上*/
template <typename T>
float DesiredStateCommand<T>::deadband(float command, T minVal, T maxVal)
{if (command < deadbandRegion && command > -deadbandRegion) //轨迹在合理范围{return 0.0;}else                                                       //轨迹在不合理范围{return (command / (2)) * (maxVal - minVal);//校正公式}
}

六、实时打印相关状态，用于调试

（1）实时打印手柄操作的状态指令


/***  功能：实时打印手柄操作的状态指令*   最后面没有调用*/
template <typename T>
void DesiredStateCommand<T>::printRawInfo()
{//（1）增量打印迭代printIter++;// （2）按要求的频率打印if (printIter == printNum) {std::cout << "[DESIRED STATE COMMAND] Printing Raw Gamepad Info...\n";std::cout << "---------------------------------------------------------\n";std::cout << "Button Start: " << gamepadCommand->start<< " | Back: " << gamepadCommand->back << "\n";std::cout << "Button A: " << gamepadCommand->a<< " | B: " << gamepadCommand->b << " | X: " << gamepadCommand->x<< " | Y: " << gamepadCommand->y << "\n";std::cout << "Left Stick Button: " << gamepadCommand->leftStickButton<< " | X: " << gamepadCommand->leftStickAnalog[0]<< " | Y: " << gamepadCommand->leftStickAnalog[1] << "\n";std::cout << "Right Analog Button: " << gamepadCommand->rightStickButton<< " | X: " << gamepadCommand->rightStickAnalog[0]<< " | Y: " << gamepadCommand->rightStickAnalog[1] << "\n";std::cout << "Left Bumper: " << gamepadCommand->leftBumper<< " | Trigger Switch: " << gamepadCommand->leftTriggerButton<< " | Trigger Value: " << gamepadCommand->leftTriggerAnalog<< "\n";std::cout << "Right Bumper: " << gamepadCommand->rightBumper<< " | Trigger Switch: " << gamepadCommand->rightTriggerButton<< " | Trigger Value: " << gamepadCommand->rightTriggerAnalog<< "\n\n";std::cout << std::endl;//重置迭代计数器printIter = 0;}
}

（2）实时打印躯干轨迹的状态

/*** 功能：实时打印躯干轨迹的状态*/
template <typename T>
void DesiredStateCommand<T>::printStateCommandInfo()
{//增量打印迭代printIter++;//按要求的频率打印if (printIter == printNum) {std::cout << "[DESIRED STATE COMMAND] Printing State Command Info...\n";std::cout << "---------------------------------------------------------\n";std::cout << "Position X: " << data.stateDes(0)<< " | Y: " << data.stateDes(1) << " | Z: " << data.stateDes(2)<< "\n";std::cout << "Orientation Roll: " << data.stateDes(3)<< " | Pitch: " << data.stateDes(4)<< " | Yaw: " << data.stateDes(5) << "\n";std::cout << "Velocity X: " << data.stateDes(6)<< " | Y: " << data.stateDes(7) << " | Z: " << data.stateDes(8)<< "\n";std::cout << "Angular Velocity X: " << data.stateDes(9)<< " | Y: " << data.stateDes(10) << " | Z: " << data.stateDes(11)<< "\n";std::cout << std::endl;std::cout << std::endl;//重置迭代计数器printIter = 0;}
}template class DesiredStateCommand<double>;
template class DesiredStateCommand<float>;

总结

xy线速度、yaw角速度可以直接由遥控手柄经过限幅给定【防盗标记–盒子君hzj】

xy位置、yaw角度姿态，是由手柄对应速度积分+状态估计器的位置信息反馈，两者一起计算的

【四足机器人--控制指令输入及转换】（1）遥控手柄状态指令转换为机器人躯干状态输入代码解析相关推荐

php把时间戳转换成字符串,php中将时间差转换为字符串提示的实现代码
如微博这看起来更加人性化,好吧,上代码 class timeAgo { static $timeagoObject; private $rustle; private $unit; private ...
【四足机器人步态状态FSM管理】电机失能模式、躯干翻身\收腿\恢复站立步态、WBC平衡站立步态、站立过程步态、对角小跑的运动步态
系列文章目录提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理文章目录系列文章目录前言一.电机失能模式SM_State_Passive 功能 1.onEn ...
四足机器人|机器狗|仿生机器人|多足机器人|Adams仿真|Simulink仿真|基于CPG的四足机器人Simulink与Adams虚拟样机|源码可直接执行|绝对干货！需要资料及指导的可以联系我！
四足机器人|机器狗|仿生机器人|多足机器人|基于CPG的四足机器人Simulink与Adams虚拟样机|源码可直接执行|绝对干货!需要资料及指导的可以联系我!QQ:1096474659 基于CPG的四 ...
四足机器人——12自由度舵机狗DIY（二）
目录一.四足机器人步态研究控制的现状 1.1目前的三种控制策略 <1>基于静态稳定的控制方法. <2>基于动力学模型的控制方法. <3>基于生物所具有的神经性调节 ...
【机器人学、机器人控视觉与控制】四足机器人MATLAB仿真
文章目录 [机器人学.机器人控视觉与控制]四足机器人MATLAB仿真 1 创建一条机器人腿 2 单腿运动 3 四腿运动 [机器人学.机器人控视觉与控制]四足机器人MATLAB仿真我们的目标是创建一个 ...
史上最全 2019 ICRA顶会四足机器人文献整理
史上最全 2019 ICRA顶会四足机器人文献整理一.ICRA论文集中相关文献对应subsession时间二.文献整理内容一.ICRA论文集中相关文献对应subsession时间 15:15-1 ...
实例10：四足机器人运动学逆解可视化与实践
实例10: 四足机器人运动学逆解单腿可视化实验目的了解逆运动学的有无解.有无多解情况. 了解运动学逆解的求解. 熟悉逆运动学中求解的几何法和代数法. 熟悉单腿舵机的简单校准. 掌握可视化逆向运动学 ...
四足机器人技术及进展
四足机器人技术及进展摘要:自然界中有许多地形无法使用传统轮式或履带式车辆到达,而哺乳动物却能够在这些地形行走自如,这充分展示出四足移动方式的优势.四足机器人已经成为当今移动机器人研究的热点,其研 ...
【论文解读--力控电机+四足机器人载体设计】（Cheetah mini）A Low Cost Modular Actuator for Dynamic Robots
系列文章目录提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理文章目录系列文章目录前言一.简介二.电机执行器类型 1.力控电动电机执行器 2.液压执行机 ...

【四足机器人--控制指令输入及转换】（1）遥控手柄状态指令转换为机器人躯干状态输入代码解析