位置控制流程图
2022-02-16 18:39:35 2 举报Pixhawk位置控制流程
作者其他创作
大纲/内容
重置所有变量，姿态设定值除偏航角外都设置为0，偏航角设定值设置为当前偏航角，公告及发布姿态设定值
_vel_sp(0) = current.vx_vel_sp(1) = current.vy
D向速度、加速度滤波处理
否
由current中的经纬度得到相对于参考位置的位移curr_sp
_vel_sp(2) = req_vel_sp_scaled(2)_pos_sp(2) = _pos(2)_run_alt_control = false
函数start()
未使能垂直速度控制时，直接将遥控油门信号乘以标度因数作为推力设定值
_pos_sp(0) = current.x_pos_sp(1) = current.y_run_pos_control = true
是
当使能了垂直速度控制但未使能水平速度控制时，要进行D向推力补偿，处理如下：
第一次进入该函数时，将当前位置/高度置为期望位置高度
thrust_sp += 0.5*dt在后面control_position()函数中会使用该设定值
cos_ratio = (ft_vel * _vel_sp) /( |ft_vel| * |_vel_sp| )得到目标速度与机体设定速度之间的夹角
是否为手动控制
NED坐标系下推力设定值thrust_sp的计算，如下：
_vel_sp= (_pos_sp - _pos) * PP为比例因子（0.95 0.95 1）
函数control_auto(dt) 单独列出部分
限制最大推力，优先保证D向推力
函数内部
函数 limit_acceleration()用来限制水平和竖直加速度
_task_should_exit=ture
向量点乘得到向量(pre--curr)与(pos--curr)之间的夹角b
4
分offboard和auto两种模式，思路都是由速度设定值得到推力和姿态设定值
机体y向的方向余弦body_y = body_z % body_x
函数 parameters_update()
起飞模式下采用jumped takeoffs中计算得出的推力
thrust_sp = P*e(v) + D*dv/dt + _thrust_int
未使能位置/高度控制的相关处理
若NE向推力设定值大于thrust_xy_max，则固定D向推力不变，减小NE向推力，以此达到降低倾斜角的目的
使能了速度、或者加速度控制
_vel_sp = 0
NE向速度小于0.8，且遥控输入(速度设定值)小于0.1
函数 cross_sphere_line()
标识变量_do_reset_alt_pos_flag为ture 表明在由自动控制切换到手动控制时，重置位置和高度以使得任务中断，飞机保持在原地； 但如果机体正处于起飞或者定点状态时，则不重置
_vel_sp(0) = max( ft_vel(0)，_vel_sp(0) )_vel_sp(1) = max( ft_vel(1)，_vel_sp(1) )
由vehicle_attitude_setpoint中读取的上次姿态设定中的数据(四元数，电机应该生成的推力T)反推得到本次计算使用的的速度设定值_vel_sp，依据如下公式： R * (0 0 -T)T= P*e(v) + D*dv/dt + _thrust_int 其中：e(v) = _vel_sp - _vel dv/dt 表示微分项，前面已计算得出； R 为由四元数得到的旋转矩阵； P、D分别为比例、微分因子；_thrust_int 为推力积分
_pos_sp(0) = _pos(0)_pos_sp(1) = _pos(1)
curr_sp_s = curr_sp * scaleprev_sp_s = prev_sp * scalepos_s = _pos * scale
由next中的经纬度得到相对于参考位置的位移next_sp，进而得到缩放值：next_sp_s = next_sp * scale
检查是否由非速度控制模式切换到了速度控制模式，是的话为保证姿态设定的连续性，做如下处理(浅蓝色框里的)：
thrust_sp等于current中的加速度
_vel_sp(0)=current.vx_vel_sp(1)=current.vy
降落过程的处理：主要是对飞机降落状态的检测(正在下降还是已经落地)，依据不同的状态对油门上下限进行处理
该函数主要作用是由速度设定值得到推力设定值，再由推力设定值得到姿态设定值(旋转矩阵)，处理如下：
怠速状态，除保留偏航角信息外，其余姿态设定值都为0，并发布数据
如果pos在向量k上的投影在向量延长线上，即curr在pos之前，则pos_sp_s= j 其中：ij=sqrt(1-d2)
函数 parameters_update()拷贝配置参数到结构体_params中
ft_vel+= ft_vel / |ft_vel| * 1.5即投影加上1.5
如果pos在k向量上的投影在curr点之后，则pos_sp_s = curr
_pos_sp(2) = _pos(2)
将位置设定值_pos_sp、速度设定值_vel_sp以及偏航角发布到主题vehicle_local_position_setpoint
依据_reset_pos_sp和_reset_alt_sp的值决定是否将当前位置/高度设定为期望位置/高度
req_vel_sp的限制性处理，防止其模超过1
起飞离地时，采用特殊的升力计算方法，如下：
while()循环
prev_curr_s = curr_sp_s - prev_sp_scurr_pos_s = pos_s - curr_sp_s分别为prev指向curr、curr指向pos的向量
设读取自vehicle_attitude_setpoint的偏航角设定值为yaw，则机体x向(机头指向)在NE面的投影向量为(cos(yaw) sin(yaw) 0)，绕D向顺时针旋转90度后得到向量： y_C = ( -sin(yaw) cos(yaw) 0 )
使能了加速度控制且_pos_sp_triplet中current加速度有效
cos_a_curr_next = prev_curr_s_norm * curr_next_s= |curr_next_s| * cos(a)a为pre--curr和curr--next的夹角
by 蜗牛拉火车
如果curr与next的距离大于单位半径，则：cos_a_curr_next=cos(a)
函数control_position(dt)
位置设定值pos_sp_s= curr_sp_s + F
使能了高度控制或者位置控制或者水平速度或者竖直速度或者加速度控制
由_pos_sp_triplet中数据获取偏航角
配置轮询主题vehicle_local_position中数据的阻塞时间为20ms
将主题vehicle_local_position中的当前的位置和速度信息拷贝到变量_pos和_vel中
偏移值小于规定值，或者遥控输入是使其(偏移)减小时，用于发布的偏航角设定值：_att_sp.yaw_body = yaw_target
使能了水平速度控制或者加速度控制
偏航角偏差 yaw_error =_att_sp.yaw_body - _yaw其中：_yaw为当前偏航角
当d1时
单独列出
根据需要，重置xy向推力积分 _thrust_int(0) _thrust_int(1)为0
current中速度为地理坐标系下的速度
使能了高度控制
缩放因子scale=(0.19 0.19 0.33)或由current中数据给出
使能了水平速度控制
函数reset_pos_sp()依据_reset_pos_sp决定是否将当前位置设置为位置设定值
机体降落
该函数主要作用是将遥控输入为速度设定值，由速度设定值得到推力设定值，进而得到姿态设定值
确保_takeoff_jumped 标志位的正确
除以缩放因子得到真实的位置设定值_pos_sp = pos_sp_s / scale
机体z向的方向余弦即推力设定值的合成向量(方向相反)，即：body_z = -thrust_sp / |_thrust_sp|
该函数在第一次进入while循环时执行，将位置解算中在第一次读取到的GPS数据作为参考位置_ref_pos，将位置设定值(如果有数据的话)重新投影到该参考位置下
函数task_main_trampoline()
函数 update_ref()
如上图：当d1时
由旋转矩阵R得到对应的滚转角att_sp.roll_body和俯仰角_att_sp.pitch_body
如果pos在向量k上的投影在向量上，即curr在pos之后，则pos_sp_s= curr_sp_s
更新速度微分dv/dt
利用如下插值法对current点进行修正，以保证航点的平滑(具体原理不懂，希望懂的大神可以指点)
由D向推力和允许的最大倾斜角计算得出允许的最大NE向推力：thrust_xy_max = -thrust_sp(2) * tan(tilt_max)
电机需生成的推力T = | thrust_sp |用于发布
由curr指向next的向量：curr_next_s = next_sp_s - curr_sp_s
将速度设定值_vel_sp发布到主题vehicle_global_velocity_setpoint中
起落架的相关设置
函数结束
函数 control_manual(dt)
函数task_main()
根据需要，重置z向推力积分 _thrust_int(2)为最小升力或者盘旋升力
函数control_offboard(dt)
_vel_sp(2) = current.vz
俯仰角、滚转角设定值为0，保留偏航角
ft_vel=0
发布计算得出的姿态设定值到主题vehicle_attitude_setpoint，包括推力设定值，欧拉角，四元数等
req_vel_sp(0) = _manual.xreq_vel_sp(1) = _manual.y
这块不知道为什么这么处理，希望懂得大神指点一下
目标速度在机体速度上的投影ft_vel = ft_vel*cos_ratio
重置所有设定值
默认采用位置/高度控制_run_pos_control = true_run_alt_control = true
该函数主要用于手动控制中偏航角控制，以及未使能速度控制时的控制方法(手动控制默认为速度控制)
手动控制
函数 control_non_manual(dt)
current中速度为机体坐标系下的速度
订阅以下主题：position_setpoint_triplet vehicle_status vehicle_control_mode control_statevehicle_local_position vehicle_land_detectedvehicle_attitude_setpoint parameter_updatemanual_control_setpoint actuator_armedvehicle_local_position_setpointvehicle_global_velocity_setpoint
_pos_sp_triplet中current数据有效
函数generate_attitude_setpoint()
a为锐角且b也为锐角
由previous中的经纬度得到相对于参考位置的位移prev_sp
当D向积分thrust_int(2)大于0时，thrust_int(2)=0，即保证D向积分不大于0，防止机体在落地后翻转
_thrust_int(2) = thrust_sp - 0.15_vel_sp(2) = -1.5
未使能水平速度控制时，遥控俯仰/滚转信号乘以标度因数，而后融合偏航角偏差计算得出俯仰角/滚转角设定值，计算如下：
_run_pos_control = true_run_alt_control = true
结合next，previous，current数据，对当前位置设定值进行修正，处理方法如下：
当curr与当前位置点的距离大于单位半径时
_vel_sp(0) = eq_vel_sp_scaled(0)_vel_sp(1) = eq_vel_sp_scaled(1)_pos_sp(0) = _pos(0)_pos_sp(1) = _pos(1)_run_pos_control=false
依据_reset_alt_sp的值决定是否将当前高度设定为期望高度
如果是FOLLOW_TARGET模式，执行如下操作：
读取主题manual_control_setpoint_s中RC的输入信号( _manual.x _manual.y _manual.z)，分别表示俯仰（[-1 1]）、滚转（[-1 1]）、油门信号（[0 1]）
偏航角偏移yaw_offs = yaw_target - 当前偏航角
第一次进入或者由自动切换到手动控制时，重置当前偏航角为偏航角设定值
计算循环周期dt
在向量pos--curr上找一点r，使得|r-pos|=1，进而pos_sp_s = r
偏航角控制
body_x body_y body_z分别为机体三轴在NED坐标系下的投影，因此由机体坐标系到NED坐标系的旋转矩阵： R=[body_x；body_y；body_]具体原理可参照 捷联式惯性导航原理（袁信、郑锷著）P51
防止速度设定值_vel_sp超限的相关处理
如果pos在k向量上的投影在prev点之前，则 pos_sp_s = prev
body_x 归一化
限制最大倾斜角，处理如下：
使能了垂直速度控制
_pos_sp(2) = current.z_run_alt_control = true
thrust_int = sum( vel_err * dt * I )
函数update_velocity_derivative()
prev_curr_s单位化，得到单位向量prev_curr_s_norm
yaw_target=_att_sp.yaw_body + w*dt
依据_reset_pos_sp的值决定是否将当前位置设定为期望位置
_vel(2) -0.75即上升速度小于0.75
由_pos_sp_triplet中数据更新偏航角，用于发布
req_vel_sp_scaled= R * req_vel_sp * 标度因数得到NED坐标系下的实际的速度设定值（后面有单独说明）
函数control_auto(dt)
机体x向的方向余弦body_x = y_C % body_z
thrust_sp * 9.81 用于向主题vehicle_local_position_setpoint中发布加速度数据
extern \"C\" __EXPORT int mc_pos_control_main指明函数入口
由旋转矩阵得到四元数和欧拉角，用于发布
D向速度小于0.6，且遥控输入(速度设定值)等于0
_att_sp.roll_body = _manual.y * 标度因数_att_sp.pitch_body = -_manual.x * 标度因数
读取_pos_sp_triplet中current、previous、next中的数据
将油门信号_manual.z加一控制死区，并限制在[-1 1]区间内，处理结果赋值给req_vel_sp(2)，处理后结果如下：
如果是刚解锁，重置各设定值、积分项及偏航角
更新推力积分项，计算如下：
机外模式
限制D向升力的最小值
使能了位置控制
cos_ratio0即夹角为锐角
函数 do_control(dt)
函数reset_alt_sp()依据_reset_alt_sp决定是否将当前高度设置为高度设定值
结合上次循环得到的速度设定值，对本次速度设定值进行限制性处理，防止两次循环间速度变化过大
mc_pos_control_main.cpp流程图
由机体坐标系转换到地理坐标系下： _vel_sp(0) = cos(yaw)*current.vx-sin(yaw)*current.vy _vel_sp(1) = sin(yaw)*current.vx+cos(yaw)*current.vy不明白为什么可以忽略俯仰/滚转角
未使能水平/垂直速度控制的相关处理
_pos_sp_triplet中current位置信息有效
由推力矢量(结合偏航角设定值)计算姿态设定值，方法如下：
5
| curr_pos_s |1表示curr与当前位置点的距离小于单位半径
thrust_sp(2) *= att_comp
vel_err = _vel_sp - _vel
函数 poll_subscriptions()
降落时，采用固定速度_vel_sp(2) = 0.5_run_alt_control = false
如果pos在k向量上的投影在prev和curr之间，则 pos_sp_s = i
由遥控输入的偏航信息得到偏航角速度设定值w = _manual.r * 标度因数
函数poll_subscriptions()拷贝各主题中的值
跳出函数control_position
0 条评论
下一页
