-
Notifications
You must be signed in to change notification settings - Fork 22
/
SolverMPC
110 lines (101 loc) · 7.76 KB
/
SolverMPC
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
参考论文：
[1] J. Di Carlo, P. M. Wensing, B. Katz, G. Bledt and S. Kim, "Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control," 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, 2018, pp. 1-9, doi: 10.1109/IROS.2018.8594448.
[2] High-slope terrain locomotion for torque-controlled quadruped robots Focchi M., del Prete A., Havoutis I., Featherstone R., Caldwell D.G., Semini C.(2017) Autonomous Robots, 41 (1) , pp. 259-272.
变量：
Matrix<fpt,13,1> x_0 >>>>>>> 初始状态向量
Matrix<fpt,3,3> I_world >>>>>>> 世界坐标系下的惯性矩阵
Matrix<fpt,13,13> A_ct >>>>>>> 参考文献[1]中的公式15中的矩阵A
Matrix<fpt,13,12> B_ct_r >>>>>>> 参考文献[1]中的公式15中的矩阵B
Matrix<fpt,13,1> full_weight >>>>>>> 权重向量
Matrix<fpt,Dynamic,1> X_d >>>>>>> MPC预测跨度中每个采样点的参考状态(期望值)
函数：
>>>>>>> void solve_mpc(update_data_t* update, problem_setup* setup)
函数功能：求解MPC问题
根据形参update和setup的成员更新rs的成员
创建rpy矩阵(3*1)存储机体的横滚角、俯仰角和偏航角
构建初始状态向量x_0
根据此笔记参考论文[1]中的公式15计算世界坐标系下的惯性矩阵
调用ct_ss_mats函数，计算A_ct和B_ct_r
调用c2qp函数，生成QP问题的矩阵A_qp和B_qp
S功能存疑？？？
使用形参update结构体中的traj数组更新参考状态向量X_d
U_b为约束上限
f_block为构建约束不等式中的系数矩阵的元素
fmat为约束不等式中的系数矩阵，可以看作是一个对角阵，对角线上的元素是f_block。这里可以参考参考此笔记参考论文[2]中的公式(8)
根据此笔记的参考论文[1]中的公式(31)(32)计算矩阵qH和qg
实例化QpProblem对象jcqp
根据形参update中的use_jcqp成员的值进行判断：
如果use_jcqp为1：
jcqp.A = fmat >>>>>>> 20h*12h
jcqp.P = qH >>>>>>> 12h*12h
jcqp.q = qg >>>>>>> 12h*1
jcqp.u = U_b >>>>>>> 20h*1
jcqp.l = 0向量 >>>>>>> 20h*1
如果use_jcqp不为1：
调用matrix_to_real函数，分别将：
qH矩阵转换为数组H_qpoases
qg矩阵转换为数组g_qpoases
fmat矩阵转换为数组A_qpoases
U_b矩阵转换为数组ub_qpoases
数组lb_qpoases全部为0.0
num_constraints为约束不等式的数目，值为20*horizon：预测跨度为horizon，每一步需要对四个足端接触力进行约束，每个力的约束有五项约束
num_variables为状态量的数目，值为12*horizon：预测跨度为horizon，每一步的状态量为12个
new_vars = num_variables，new_cons = num_constraints
字符数组con_elim为全0数组，数组长度为num_constraints
字符数组var_elim为全0数组，数组长度为num_variables
构建循环体，循环次数为num_constraints次：
对约束不等式的上下界进行遍历检查，如果两个边界都在0附近，意味着这一采样点的这一条腿处于悬空状态，所以GRF为0，则进行接下来的循环体。如果两个边界有一个不在0附近，则直接进行下一次循环
两个边界值都为零的行所对应的数组起始元素位置地址存储在c_row中
遍历A_qpoases数组从c_row开始的num_variables个元素，相当于遍历fmat矩阵第i行的所有元素
调用函数near_one判断遍历的元素是否在1附近，如果在1附近，即定位悬空腿的z方向的力对应的约束矩阵A_qpoases的系数
new_vars中存储着地腿的变量数目(3的倍数)
new_cons中存储着地腿的约束数目(5的倍数)
将全0且长度为num_variables的数组var_elim中，悬空腿对应的3个元素置1；将全0且长度为num_constraints的数组con_elim中，悬空腿对应的5个元素置1
构建长度为new_vars的数组var_ind
构建长度为new_cons的数组con_ind
构建循环体，循环次数为num_variables次
var_ind存储着地腿的变量序号
构建循环体，循环次数为num_constraints次
con_ind存储着地腿的约束序号
构建循环体，循环次数为new_vars次
数组g_red存储g_qpoases中位于着地腿编号位置的数值
数组H_red存储H_qpoases中位于着地腿编号位置的数值
构建循环体，循环次数为new_cons次
数组A_red存储A_qpoases中位于着地腿编号位置的数值
构建循环体，循环次数为new_cons次
数组ub_red存储ub_qpoases中位于着地腿编号位置的数值
数组lb_red存储lb_qpoases中位于着地腿编号位置的数值
上面这几个步骤总结一下：
最初构建的QP问题，包括了着地腿和悬空腿，但是最后只能对着地腿进行规划，因此，根据约束不等式中上界数组ub_qpoases和下界数组lb_qpoases中的元素进行遍历，如果发现在某一位置二者同时很接近0,则认为在这一采样点处，这个方向的力不存在，也就意味着在这一时刻这条腿悬空。接下来根据悬空腿和着地腿的编号，将原先QP问题中的数组中对应的悬空腿的部分剔除，最后只剩下着地腿的部分。
如果use_jcqp为0：
构建基于qpOASES求解器的求解方法，参考：https://blog.csdn.net/weixin_40709533/article/details/86064148
完成求解，在数组q_red中存储原始解
对原始解进行处理：将悬空腿对应的最终解置0.0,着地腿和悬空腿按照最初顺序存储在q_soln中
否则(源码注释中给出的此时use_jcqp为2)：
与use_jcqp为0的情况大同小异，只是采用了另一种方式求解QP问题
如果use_jcqp为1：
与use_jcqp为0的情况大同小异，只是采用了另一种方式求解QP问题
>>>>>>> void ct_ss_mats(Matrix<fpt,3,3> I_world, fpt m, Matrix<fpt,3,4> r_feet, Matrix<fpt,3,3> R_yaw, Matrix<fpt,13,13>& A, Matrix<fpt,13,12>& B, float x_drag)
函数功能：计算参考文献[1]中的矩阵A和B
调用函数cross_mat，为计算力矩做准备
x_drag存疑：z方向的机体速度 = x_drag * x方向的机体速度 - 9.8 + z方向的合力/质量
>>>>>>> inline Matrix<fpt,3,3> cross_mat(Matrix<fpt,3,3> I_inv, Matrix<fpt,3,1> r)
函数功能：将形参r从向量转换为叉乘矩阵，并与形参I_inv相乘
调用时，I_inv为机体在世界坐标系下的惯性矩阵，r为足端坐标的叉乘矩阵
>>>>>>> void c2qp(Matrix<fpt,13,13> Ac, Matrix<fpt,13,12> Bc,fpt dt,s16 horizon)
函数功能：生成QP问题的矩阵
ABC为25*25的方阵，其中左上角13*13的块存储Ac，右上角13*12的块存储Bc，并与采样时间间隔dt相乘
expmm相当于是求解一个齐次微分方程的解，具体的原理参考：https://blog.csdn.net/HaoZiHuang/article/details/105024959
拆分expmm矩阵，左上角的13*13的块赋值给Adt，右上角的13*12的块赋值给Bdt
至此，完成系数矩阵的离散化
定义包含20个元素的矩阵数组powerMats，其中每个元素是接下来长度为horizon预测跨度中，每个采样点的Adt(相对于初始状态，而不是上一状态)
A_qp中从上到下依次为：Adt、Adt*Adt、Adt*Adt*Adt ......，即每一采样时刻相对于初始状态的状态转移矩阵
B_qp同理为下三角矩阵
>>>>>>> void matrix_to_real(qpOASES::real_t* dst, Matrix<fpt,Dynamic,Dynamic> src, s16 rows, s16 cols)
函数功能：将rows*cols维度的矩阵转换为数组dst，逐行扫描
>>>>>>> s8 near_zero(fpt a)
函数功能：判断输入参数a的范围，如果a大于-0.01且小于0.01,返回1；否则返回0
>>>>>>> s8 near_one(fpt a)
函数功能：判断输入参数a的范围，如果a大于0.99且小于1.01,返回1；否则返回0
>>>>>>> mfp* get_q_soln()
函数功能：返回MPC问题的解向量q_soln